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ABSTRACT 


Autonomous Underwater Vehicles (AUV) are being 
considered by the U.S. Navy for a variety of missions. 
Requirements for autonomy reinforce the need for a robust 
maneuvering controller that can ensure accurate tracking of 
a planned path. Model reference controllers (MRC) have been 
employed in situations where accurate tracking is desired 
and where plant parameters change with operating conditions. 
Because underwater vehicles are highly non-linear, it is 
conjectured that an MRC will provide improved tracking 
performance for AUVs. This thesis presents the results of a 
Simulation study in which the dynamics of a submersible are 
modeled using a modified version of the DTNSRDC 2510 
equations of motion. A linearized version of these 
equations serves as the reference model and provides the 
basis for the design of feedforward and feedback elements of 
the controller. Results show that for dive plane maneuvers, 
accurate tracking of the planned path can be achieved for a 


moderately wide range of vehicle speeds. 
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I. INTRODUCTION 


A. GENERAL 

In recent years, the focus on Autonomous Underwater 
Vehicles (AUV's) or, more generally, Unmanned Underwater 
Vehicles (UUV's) has increased. A variety of unclassified 
missions includes Search and Survey, Decoy and Outboard | 
sensors, Ocean Engineering Work Service, Swimmer Support, 
and Test and Evaluations [{Ref. 1]. As the cost of manned 
submarine vehicles increases, there are significant 
advantages to the use of cheaper unmanned vehicles. UUV's 
can be either tethered or untethered. Development in both 
areas is proceeding, but, while tethered vehicles can use 
fiber optic links to human operators on a mother ship, a 
fully autonomous vehicle is required to have a high level of 
intelligent processing on board. Thus the requirements for 
AI and Knowledge-Based Controls are much increased. A 
recent symposium [{Ref. 2] has presented a summary of the 
State of the Art in Unmanned Untethered Submersible 
Technology. 

The organization of the intelligent control of an AUV 
can be expressed as a eeie of Sensing, Thinking, and Acting 
(Figure 1.1). At the highest level of the control 
architecture, the mission planning and symbolic reasoning 
lead to requirements for path planning and control. The 


lower level of Acting involves operation and control of all 


THINKING 


PERCEPTION AND PLANNING AND 
REPRESENTATION CONTROL 


COGNITIVE STRATEGIC 


SYMBOLIC TACTICAL 


SENSING ACTING 


SENSORY REFLEXIVE 


REAL WORLD 


Figure 1.1 Organization of Intelligent Control 


vehicle modes of behavior. At the Sensing level, all 
information concerning the environment surrounding the 
vehicle, as well as its own internal state of health, is 
directed to the higher level. Figure 1.1, reproduced here 
from [Ref. 2] illustrates the idea, and Figure 1.2 
illustrates the hierarchical nature of the intelligent 
controls required. 

Part of the sensing and reflexive acting at the lowest 


level involves a high degree of servo-control over all six 





degrees of freedom of the vehicle motion. To effect proper 
control, not only must the autopilot be capable of accurate 
course and depth control, but also, commands for reflexive 

actions for avoidance or attack must be followed accurately. 
These commands can also include hovering while some form of 


underwater work is done. 


B. AIM OF THE STUDY 

This thesis is concerned with the lowest level of 
control--the control of vehicle reflexive eee Lieas 
assumed that the planning level control in Figure 1.2 
recognizes the need for evasive action and decides on 
parameters such as speed, course, and depth changes to be 
rapidly implemented. These parameters are then fed to a 
series of stored maneuvers within the framework of a model 
based autopilot system. Figure 1.3 illustrates the concept 
of the "bag" of maneuvers as interfaced to the vehicle 


autopilot. The control concept proposed here is that of a 
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Figure 1.2 Hierarchical Control Structure 
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developed for such maneuvers can be in the form of 
algorithms that provide a command generation system to the 
autopilot. 
The purpose of this work is to determine the feasibility 
and the autopilot design methodology for: 
1. the command generation logic, and 


2. a model following autopilot control. 


C. METHOD OF APPROACH 

Since this work deals strongly with underwater vehicle 
dynamics and control, but not with the vehicle hydrodynamics 
per se, it was important to use an existing vehicle model as 
the basis for the work. Such a model (Figure 1.4) was 
provided by [Ref. 4] where the verification of the model by 
experimental data illustrated the reasonableness of its 
coefficients. 

Using the equations of motions of the vehicle, the 
development of command generation logic, the design of the 
model following autopilot, and the AUV maneuvering 
performance, have been accomplished with computer 
Simulation. Heavy use of the DSL (Dynamic Simulation 
Language) has been made. [Ref. 5] 

The vehicle selected as the basis for the study is 
approximately 17 feet long and has been simulated over a 


range of speeds from 3 to 30 feet per second where a 
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specific maneuver--a rapid dive to 100 feet--has been the 
focus for the command generation model. 
While much remains to be done, the concept proposed 


appears worthy of future work. 


II. VEHICLE DYNAMIC MODELING 


A. GENERAL 

This chapter describes the dynamics of a selected AUV. 
The three dimensional motion of an underwater vehicle is 
fully defined using two coordinate reference systems. 

1. Body Fixed Coordinate Reference System--Figure 2.1. 
2. Inertial Reference System--Figure 2.2. 

The vehicle equations of motion are presented and how 
they were modified to suit the needs of an AUV. Also 
included as part of this chapter is a description of the 
derivation of the hydrodynamic coefficients and a brief 
discussion of the propulsion plant and crossflow drag 


modeling. 


B. COORDINATE SYSTEMS 

_ Three dimensional motions of underwater vehicles are 
normally described using two coordinate reference frames. 
The first is a right-handed orthogonal coordinate system 
fixed in the body. The second, an inertial reference frame, 
is used to define translational and rotational motions in 
global coordinates (Figure 2.1) 

The body fixed coordinate reference frame has its origin 
fixed to the body center and is aligned with the body axis 
of symmetry. Components of the vehicle motion relative to 


this body fixed reference frame are defined as: 
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Figure 2.1 Body Fixed Coordinate Reference Frame 
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u,V,W components along the body fixed axes of the 
velocity of the origin relative to the fluid 
(Surge, Sway and Heave velocity respectively). 

Pras components along the body fixed axes of the 
angular velocity of body relative to the 
inertial reference system (Roll, Pitch, and Yaw 
rates) (Figure 2.2). 

The inertial reference frame is also a right handed 
orthogonal coordinate system in which the position and 
orientation of the vehicle's coordinate system is specified. 
The orientation of the body-fixed coordinate system is 
described by Euler angles (yaw), 6 (pitch), 9 (roll). 
The transformation from body-fixed to inertial is then given 
conveniently by an XYZ rotation sequence ($,6,w). 

Position of the body-fixed coordinate system is then 
expressed in X, Y, and Z coordinates as illustrated in 


Figure 2.1. Orientation of the vehicle's coordinate system 


is expressed in Euler angles 9, 6, j- 


C. RIGID BODY DYNAMICS AND EQUATIONS OF MOTION 

The equations of motion for a six degree of freedom 
submarine vehicle are now standardized being first fully 
developed by Gertler and Hagen [Ref. 6]. These equations 
are commonly known today as the DTNSRDC 2510 equations of 
motion. 

Modifications to these standard equations are then 
generally made to reflect the particular hydrodynamic 
characteristics and properties of the underwater vehicle 
being considered. [Ref. 6] ‘Among the most significant 


eZ 


changes/allowances considered for the AUV in this study 
included an integral formulation of the viscous crossflow 
forces and moments, addition of the effect of an external 
current and perhaps the most significant difference is the 
change made due to the non-conventional shape of the AUV. 
The AUV considered here is peculiar in that its shape is 
more of low aspect ratio wing than that of the conventional 
body of revolution. [{Ref. 4] Additional modifications were 
also made by the separation of the coupled input for bow and 
stern planes and also the decoupling of the bow planes so 
that purposely induced roll control could be included. 

The equations of motion for the six degree of freedom 


AUV are listed in Appendix A, in the following form: 


xX — h(x, 2) (2.1) 
where, 
M = MASS MATRIX (2220) 
x = ogre! (2.3) 
f£ = [Fy,Fy,Fz,K,M,N]? (ee) 


and Fy, Fy, F, are hydrodynamic forces and K, M, N are 


hydrodynamic moments, 
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surge 
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sway 
heave = Body Coordinate States (25) 


rod 


Q SO 


Deeen 


K 


yaw 


Position 
Z = Z (236) 
Inertial Reference System 


ms 


K< 


= 


Orientation 


cD 


rudder angle 

ST&D bow plane angle 

port bow plane angle (2.7) 
COnEEol 


stern plane angle vector 


delta form 


us” if uP” oS” q 


delta buoyancy 


and u is distinguished by context from u--surge velocity of 
the vehicle relative to the surrounding water, or Ugo for 
the current. 

In addition to the six equations of motion that define 


the AUV's motion relative to-the body fixed coordinate 
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system, six additional equations are required to fully 
specify the vehicle's motion in space. These kinematic 
relations (see Appendix A) specify the position and 
orientation of the body coordinates with respect to an 
inertial reference frame as established by the XYZ 
rotations, and are expressed in terms of linear velocities 


and Euler angular rates. 


D. HYDRODYNAMIC COEFFICIENTS 

Although development of the hydrodynamic coefficients is 
not a thrust of this thesis, a brief description of their 
derivation is warranted. 

The hydrodynamic coefficients provide the source of the 
behavioral characteristics, and thus the responsiveness, of 
a particular underwater vehicle. 

These coefficients are the result of a Taylor series 
expansion, in which only the first order terms are saved, 
based on the motion variables of the hydrodynamic forces and 
moments. The hydrodynamic coefficients are non- | 
dimensionalized and can be considered constants within 
limited operating ranges. (Ref. 6] 

There are currently two primary methods utilized for 
obtaining hydrodynamic coefficients. The first is based on 
tow tank experiments using planar motion, and rotating arm 
mechanisms. The second is a geometric analytical approach 


using semi-empirical techniques. ([Ref. 4] 


cS 


The coefficients used for this thesis are those that 
were determined using the analytic approach for an SDV 
simulator. {[{Ref. 4] 

The coefficients thus selected were chosen because of 
convenience and availability rather than any particular 


desirability of the hydrodynamic characteristics implied. 


E. PROPULSION AND CROSSFLOW DRAG MODELING 


1. Propulsion Plant Modeling 


In NCSC's report by Crane, Summey and Smith [Ref. 
4}, propulsion plant modeling is discussed. In that report 
they list the effects of propulsion on the motion of a 
submersible. 

propulsion thrust 

propeller slipstream effects 

propulsive torque 

propulsion induced hull effects 
Of these four effects only the first two are considered 
substantial and the last two are considered negligible. 

The propulsive thrust equation was derived by NCSC 
by curve fitting experimental data and the propeller 
slipstream effects are modeled as a function of vehicle 
speed, propeller rpm, and geometry. [Ref. 4] 

2. Crossflow Drag Modeling 

Since the AUV geometry selected in this study is 

essentially a low aspect ratio wing design and not a body of 


revolution, its body cross-sections are nearly rectangular 
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rather than circular. Because of this an integral strip 
theory formulation of crossflow forces and moments was 
developed and incorporated into the equations of motion as 


given in Appendix A. 


F. BOW PLANE INFLUENCE 

Bow plane action serves to augment stern plane control 
over pitch motions, but adds to the hydrodynamically induced 
drag on the vehicle. When port and starboard bow planes are 
separately controlled, active control over vehicle roll 
motion may be accomplished. Thus the coefficients relating 
to the heave and pitch motions, axial drag, and roll motions 
have been modified here to allow separate active roll 


control. 


7 


III. LINEARIZATION OF THE VEHICLE EQUATIONS OF MOTION 


A. GENERAL 

The overall objective of this chapter is to fully 
describe the techniques used to linearize the highly non- 
linear equations of motion. A step by step and term by term 
development of the linearized equations are presented and 
all variables are completely specified in their relation to 
the AUV in this study. 

A description of the linearization point and the 
ramifications of linearization about a straight line path is 


also considered in this section. 


B. LINEARIZATION PROCEDURES 

Linearization of the vehicle dynamics is required for 
the design of the vehicle control system. The linearized 
equations also serve as the model reference for the 
controller. The desired form is the state space 


representation of the equations of motion given as, 


x = A Ax + B Au (3.1) 


(S 
> 


As discussed in Chapter II, the vehicle dynamics are 


represented in the following form: 


x = £(x,z,u) (3.2) 
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where M is the mass matrix, - is the time derivative of the 
state vector x and u is the input vector. For the immediate 
purpose at hand, Z may be considered to be part of the state 
vector x. Proper separation will be discussed in what 
follows. 

Linearization is accomplished by a Taylor’ series 
expansion about a nominal path or trajectory given generally 
by (Xo9(t),Uo(t)), with only the first order terms being 


retained. The following form is then obtained: 


Mexo tf MT AX = £(X5rUq) + aa 
Ginn, 1) A 
‘la a a Src 
c du ( ) 


where, if Ax = (X - Xo), and ay = (y - Uo), and Equation 


(3.3) becomes, 


o£ (x su )Ax of(x,-U,) du 


Mee = 0-0 = = =o -0 = . 3.4 
M AX ax r a0 ( ) 
df(x_,u.) df (x_,u_) 

Defining A = — and B= a the desired 


state space form is obtained. 


Gee ae PLICATION TO VEHICLE MODEL 
The state space model is a 12 state model that can be 
separated into two state vectors x and z. The state vector 


x represents the three linear velocities and corresponding 


Ue, 


three angular rates about an orthogonal coordinate system 
fixed in the body as defined in Equation (2.5). 

The state vector z represents the six kinematic 
relations, three coordinate positions and three Euler angles 
and is defined in Equation (2.6). 

The two sets of six equations that result are of the 


foi 
x = M7l£(x,z,u) (325) 
2 = g(X,z) (3.6) 
The control vector u is the input vector and is defined 
by Equation (2.7). 


By combining both state vectors, the model state vector 


is defined, 
X= (x,z])' = [(uvwpqrxvyaZ2ooeyjt : (3) 
Once the model state vector and control vector are 


defined, the A matrix and B matrix must be determined. The 


A matrix formulation is represented, 


-l —] 
M “Of (x .Z,/0,) M “of (x ,Z,,U) 
ox dz 
= 7 dg (x. zo es 
12x12 ag (X72) G(X 1Zy 
| ox dz 


20 


and by similar formulation the B matrix is, 


M “of (KZ /4)) 
ue 
B= (3.9) 
eee og (X12) 
aus 


An element by element formulation of the A and B 
Matrices are complex and require careful attention. The 
particular functional form of the derivative expressions 
can, however, be obtained analytically and depending on 
whether xX, and Zp, are time dependent or constant, the 
analytical derivatives become time variant or not. For the 
case of linearization about a straight line flight path, 
these derivatives are constant which makes the control 
computations easier than for more complicated nominal flight 


conditions. 


D. LINEARIZED VARIABLES ABOUT STRAIGHT FLIGHT PATHS 

One convenient feature concerning the linearization 
about a straight flight path with forward speed, u,, is that 
A and B become constant matrices where the coefficients are 
relatively simple functions of the forward speed. Also, 
Since the nominal path is associated with neither rotation 
nor cross-track or depth translation, the incremental 


variables Ax and Au are identical to the actual variables x 


2 


and u except for the longitudinal velocity and position. 
The linearized dynamics in the axial direction become: 


x = (3510) 


u 
—O 


ao 


so that as far as the linearized system dynamics are 
concerned Ax(1) = 0 and Ax(t) = uo(t)- While this feature 
is convenient, it does not provide information on the second 
order effect of control surface action slowing down the 
vehicle. 

A possible approach to alleviating this deficiency in 
the linear model could be to modify the axial direction 
equation of motion so that the drag effects of control 
surface action are related to |65| rather than bee This is 


beyond the scope of this thesis. 
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IV. AUTOPILOT DESIGN USING OPTIMAL 
CONTROL TECHNIQUES 
A. GENERAL 

This chapter contains a review of optimal control 
techniques as developed and used in this study for the 
control of autonomous underwater vehicles. Such an 
autopilot has been classically treated as a series of 
interconnected feedback loops for independent control of 
depth and control of course and heading, while roll control 
of the vehicle has been left passive. Control of the sixth 
degree of freedom, longitudinal velocity, has not been 
considered important and a constant thrust or propeller 
speed has been assumed. 

While control of all six degrees of freedom may be 
agmportant in the end for future AUV operations, and 
particularly in the transition from cruise to hover modes, 
this is not the primary focus here. Instead, this chapter 
deals with the state of the art in systems concepts for 
underwater vehicle course and depth control, together with a 
review of the modern multivariable system controls methods 


used in modern autopilot design. 


B. CLASSICAL CONTROL OF COURSE AND DEPTH 
Simple autopilots have long been of interest in 


relieving the human operator of onerous tasks and preventing 
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fatigue. elaesicaldaeeiee techniques have considered depth 
and course control as separate, non-interacting control 
systems. The depth controller directs commands to the stern 
planes based on an error between pitch angle command and 
vehicle pitch angle where the pitch command is proportional 
to depth error. Course heading controllers provide rudder 
angle commands proportional to heading angle error. Walker 
(Ref. 7] recently proposed the addition of a cross track 
position feedback loop using yaw angle damping to control 
the cross track distance for automatic track control. 

Most vehicle controllers in practice rely on classical 
concepts with protection limits on command signals so that 
control surface commands can be limited in magnitude and 
rate. Adaptive steering controllers have been proposed as 
an extension for course maintenance in heavy seas when 
optimized gain settings are based on calm weather ship 
characteristics [Ref. 8]. The main limitation of autopilot 
designs based on classical concepts are, 

1. Ship characteristics vary strongly with speed so that 
gain settings for all of the major loops have to be 
adjusted to maintain optimum performance under wide 
operating conditions. 

2. Gains set based on maximum actuation limits and 
usually designed to regulate vehicle depth and course 


about nominally fixed reference settings. 


3. Control of depth and course changes (i.e., rapid 
maneuvering) is not easy and usually not automated. 


The control of rapid maneuvering is more suited to the 


more recent multivariable control system structures such as 
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those involving Model Following Controllers, and Model Based 
Compensators as proposed by Milliken [Ref. 9]. 


C. REVIEW OF OPTIMAL CONTROL CONCEPTS RELATING TO 
CONTROLLER DESIGN 


1. LOR Summary and Discussion 

Much has been written about the application of 
Optimal Control Concepts to the design of feedback systems 
for both output regulation and input tracking. Kwaakernaak 
and Sivan [Ref. 10] present a discussion of design methods 
based on state of the art to 1971. Kaufman and Berry [Ref. 
11] have provided examples of autopilot design methods based 
on linear optimal regulator (LQR) methods and  =model 
following techniques. Milliken [Ref. 9] has’ showed 
recently, the use of Model Based Compensators for providing 
multi-degrees of freedom control for a submarine depth and 
course control using linear control techniques--similar to 
those used in this work. Most recently, non-linear control 
methods have been proposed by Slotine [Ref. 12], and Yoeger 
and Slotine [Ref. 13] to provide robust trajectory control 
for underwater vehicles. Using linear control procedures, 
the vehicle, or object to be controlled is described by a 
linear state variable dynamic model for response computation 


by equations of the forn, 


Xp(t) = Ap Xp(t) + Bp up(t)?  X%p(0) given (4.1) 
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in which the matrices Ap and Bo represent constant 
coefficient terms, and Xp (t) and Up (t) respectively, 
represent the vector of motions (positions and velocities) 
and the control actions (control surface deflections). 

~The design of a linear optimal regulator (LQR) 
control is based on the notion that if some non-zero initial 
condition, x(0), is established, then Up (t) can be designed 
so that the non-zero state values can be reduced to the 


equilibrium values x(t) = 0, x(t) = O with a_ control 


operation given by, 


Up (tC) es Xp (t) (4.2) 


where K is found from the minimization of the quadratic 


performance index, 


J = f (Pox+u? Ru) dt (4.3) 
Here, © iS a non-negative definite square symmetric 
weighting matrix for response magnitudes and R is a positive 
definite square symmetric weighting matrix for control 
effort. © is size nxn, and R has rank equal to the number 
of control inputs modeled (r). 
The solution for K becomes a matrix of size rxn 


found as the solution to, 
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K = RT! Bp? p (4.4) 
where 
PA+ AR +0 - R(B KB‘) B= 0 (4.5) 
The eigenvalues of the closed loop regulator are 


determined from the combined state and co-state system 


equations. They are given by the eigenvalues of the 


composite matrix SS where, 


> 
[vs 
i 
~ 
ey) 
ry 


It can be show [{Ref. 10] that P is also given by, 


P = [Wo] (W,)7+ | (47) 


where Wi, Wp are the nxn partitions of the matrix, W, 


Wa 
Wa 
formed from columns of stable eigenvectors of SS. It has 


also been found that the use of real part and imaginary 
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parts of a complex conjugate eigenvector as adjacent columns 
of W where a complex pole pair exist, eliminates the need 
for complex matrix inversion [Ref. 10]. 

The design by minimization of J in Equation (4.3) 


yields the closed loop control system equations, 
Xp (TC) = ae K) Xp (t) ; Up eae) Xp! Xp (0) given (4.9) 


where the steady state response is zero for both Xp and Up: 
The state vector may, in many cases, be considered 
as a deviation vector from a desired constant level, and it 
is quite appropriate for the steady state values of Xp and 
up to go to zero. However, in the reality of some cases, 
the maintenance of a constant level in some elements of the 
state vector requires a non-zero steady state control signal 
level and in these cases Up (©) = 0. If a steady state level 
must be maintained for any element of the state vector, the 


steady state equations are first solved as, 


= Ap Xp (©) ti Bo Up (>) (4.10) 


Equation (4.10), subtracted from Equation (4.1) 
reduces these cases to the eguivalent of a regulator control 


problem by shift of variable, 
Xp(t) = Ap(Xp(t) - Aplo)) + Bo(Up(t) - Uple)) oe 
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where the new variables 


Xp(t) - Xp(o) (4.12) 


and 


Up(t) - Up (o) | (4.13) 


are related in a control law 


Up (t) -— Upl@) = -K(Xp(t) - Xp(~)) (4.14) 


or 


Up(t) = Ulm) - K(Xp(t) - Xp(-)) (4.15) 
Bp Bp Bp Bp 


The above discussion has been limited to 
deterministic signals and to the assumption that all 
physical state variables are either measurable or determined 
in a full state observer [Ref. 10]. 

Where the output of the controlled process is to be 
regulated, the above techniques may be used to design the 
elements of the feedback gain matrix thus avoiding the 
complex task of designing separate control loops from each 


variable in the process. The method is powerful, but 
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requires skill in the selection of appropriate 9 and R 
factors. 
2. Tracking Control Systems--(LMFC + MRAC 

Where the control system is required to drive a 
process so that the output tracks an input variable within 
acceptable error bounds, the problem is further compounded. 
Even more difficult is to achieve the tracking of several 
simultaneous inputs by the TaEneus outputs of the driven 
process. During the late 1960's and early 1970's, much 
attention was placed on linear model following controls 
(LMFC) and model reference adaptive controls (MRAC) to 
provide the acceptable tracking behavior of multivariable 
systems. Kaufman and Berry [Ref. 11] described the 
application to flight control, and Landau [Refs. 14,15] gave 
a survey of design techniques and system structures in which 
it became clear that a model of the system to be controlled 
was needed to represent the desired time behavior of the 
system state variables. The system control variables then 
became a function of the input variables to the model, and 
the model state variables, in addition to the feedback of 
system state variables. Thus better information than could 
be derived by feedback —T used to drive outputs to track 
inputs. 

The use of MRAC techniques allows for not only model 


following, but also the provision of adapting gains, or 
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model parameters in achieving precise control when system 
operating conditions change. 

One of the difficulties pointed out by Landau [Ref. 
14], is that controller parameters need to change when the 
plant operating conditions changed. Thus using a reference 
model not only provides the robustness achieved by 
predictive and corrective control but also provides the 
opportunity co update model and Conere! parameters 
automatically. 

Restricting the discussion to Linear Model Following 
Controls (LMFC), the control issues are analyzed as follows: 


the plant model is given by, 


x = Ap Xp + By Up (4.16) 


Yp = Lp (4.17) 


I 
SO 


and a suitable model of the plant, but with’ desirable 
dynamic response characteristics (response time, stability, 


etc.) is given by, 


Xm = Am Xm + Bm Um(*b) (4.18) 


Un = 0 (4.19) 
Ym = nm Xm (4.20) 


Bal 


then sthe Control sigmadce Up, which minimize the weighted 


integral of errors between model and plant are given by, 
Up = Ki Um + K2 Xm + K3 Xp (4.21) 
where the errors are defined as, 
€ = Ym 7 Yp (4.22) 
and the performance index minimized is, 
t= f (eT Qe + up? Rup) at (4.23) 


and Q, and R are weighting matrices as discussed earlier. 

The computation of the gain matrices, Kj, Kj, and k3 
are fortunately made easier by considering the combined 
system, model plus plant as a coupled linear system. Also, 
to overcome problems that arise when the signals to be 
tracked, y,(t) are derived from inputs, u,(t), that are not 
impulses, it is convenient to consider that the additional 
model equations, 


be incorporated together with u, as a composite state 


vector, 
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xt aa [Xp? ym - Ym (4.25) 


to get the system equations, 


: Xp An Oo 0 Xp Bp 
ce | Xm =e ee al xl at |0 | | (4.26) 
Un 0 oO 0 Un 0 


Now, application of the LQR technique to the composite 


system given above yields, 


Up = -(B7* Bol) (P](Xp Xm Um)? (4.27) 


where. P now is of dimension (np + nm + rm), and [R71 Bl Pik 


is partitioned in three parts, 


[R + Bt Pp} = [Kz Kp Kj) (4.28) 


By varying the weighting factors within the matrix, 

Q, selected errors may be penalized more heavily than others 

| in the optimal control trade-off. Also, selection of 
parameters within R may be used to provide a trade-off 
between a Sluggish or sensitive control design. Details of 
numerical values used in the design of the autopilot 


controls are given later in Chapter VI. 
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3. Near Time Optimal Maneuvering Models 


The use of the system structure implied by Equation 
(4.26) and the resulting control law, Equation (4.21), is 
Baredcullerie useful when considering near time optimal 
positioning of inertial objects. It is well known that time 
optimal position control of a massive object requires a 
bang-bang application of force or torque. These concepts 
are recently being considered in robot tracking control 
(Ref. 16], and the sliding control described in [{Ref. 17]. 
So also, in the field of LMFC for underwater vehicle 
maneuvering control, it is expected that rapid maneuvering 
will require some form of bang-bang operation of control 
surfaces. Bang-bang operation, in principle, is simple, 
consisting of a sequence of stepwise control actions, yet 
knowledge of switching times for anything other than very 
low order systems make the principle difficult to implement. 
The outcome of the above discussion then leads to the 
development of vehicle maneuvering models based on use of a 
series of constant setting for control surfaces that make Up 
the model input vector u,. At times during the response of 
the model where switching should occur, the control surfaces 
change setting rapidly as if by imposition of an impulse 
command. Therefore, if it is considered that surface 
settings change levels at discrete but arbitrary times, the 


unforced nature of the model reference states, in Equation 
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(4.26) are preserved and the application of the LMFC system 
is valid. 

For every reflexive maneuver envisioned during the 
operation of an AUV life, it is foreseen that maneuvering 
logic can be developed on an algorithmic basis to determine 
switching times, using logic to be developed and the A,, Bn, 
Ka, Ko and K3 matrices as shown in -Figure 4.1. These data 
can be stored inside on-board processors so that on command 
from the high level controller or expert system, new 
computations for Up can be implemented immediately. 

The development of a maneuvering logic as a command 
generation system for a dive maneuver will be discussed in 


more detail. 
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V. SIMULATION TECHNIQUES 


A. GENERAL 

This chapter contains a discussion of the computational 
program structures used in this study. All Simulations were 
performed using the Dynamic Simulation Language (DSL) [Ref. 
5] code for the simulation of linear and non-linear system 
response as a function of time. Internal numerical 
integration routines make this aspect of the solution 
transparent to the user. The user provides only the details 
of the particular equations employed. In this work, DSL was 
used for the sn ates of both uncontrolled and autopilot 
controlled vehicle responses. However, as part of the 
design procedure for the autopilot, the complete set of 
feedforward and feedback gains were established using ETAT-- 
a Beet ally developed program for the computation of linear 
optimal control gains. The pertinent linkages between DSL 
and ETAT were developed and implemented during this study. 


More detailed descriptions follow. 


B. COMPUTATION OF FEEDFORWARD AND FEEDBACK GAINS 

While the theory behind the need for feedforward gains 
for optimal model following autopilots has been given in 
Chapter IV, this section discusses the program organization 


used in their computation. 
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The outline organization of program ETAT is shown in 
Figure 5.1. ETAT reads and writes values of AA, and BB, as 
computed within the framework of the DSL simulation and also 
reads user input values for the tracking error weighting 
matrix, Q, and the control input weighting matrix, R. 
Particular values used for 9, and R, are given later in 
Chapter VI. 

Subroutine MTXEXP computes the matrix exponential 
associated with AA, and the discrete time input matrix 


associated with AA and BB, but this section has not been 
used here. 

Subroutine ROOTS is used for the computation of both 
eigenvalues and eigenvectors of a square matrix (AA), and 
calls the IMSL double precision library routine EIGRF and 
its associated subroutines. 

OPTIMA is the subroutine used for assembly of the 
| composite state and co-state matrix, SS. OPTIMA also calls 
EIGRF and computes the closed loop system eigenvalues and 
vectors. These, as given earlier in Chapter IV, are used to 
form the solution of the matrix Ricatti equation and the 
overall matrix of gains, i.e., Equation (4.4). Partitions 
of the overall gain matrix give the individual matrices, Ky, 
Ky and K3 in Equation (4.28). 

A listing of the major subroutines used in program ETAT 


are provided in the appendix for the interested reader, 


although use of ETAT without. proper linkages to DSL and the 
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INPUT 


READ AA, BB, Q, R 


WRITE TO OUTPUT 


| MTXEXP 
AAAt 
ETAT COMPUTE, e 


MAIN 


PROGRAM 
ROOTS 


COMPUTE EIGENVALUES / 
EIGENVECTORS, AA 


OPTIMA 


SET UP SS MATRIX, 
COMPUTE EIGENVALUES/ 
EIGENVECTORS 











FORM SOLUTION FOR 
FEEDFORWARD/FEEDBACK 
GAINS, Ki » Ko, K3 


FORM CLOSED LOOP 
AA MATRIX 
CALL ROOTS 


CHECK CLOSED LOOP 
EIGENVALUES 





Figure 5.1 ETAT Flow Diagram 
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appropriate IMSL double precision library would not be 


proper. 


C. REFERENCE MODEL DEVELOPMENT 
As discussed earlier, the reference model is a full 12 
state representation of the AUV. The reference model can be 


represented by: 
Xm = AA Xm + BB u Un = 0 (5.1) 


A computational problem in subroutine OPTIMA can arise 
because of the multiple zero eigenvalues associated with 
several of the modes in the above equations. MThis problem 
has been overcome here by inserting very small values,-: 
~(A);, on the key diagonal elements of the AA matrix so that 
distinct eigenvalues result. Since the (dA) values are 
extremely small, their effect on the system poles is 
negligible and the problem of multiple repeated poles is 
eliminated. 

| It is conceivable to have a series of reference models, 
one for each of several reflexive maneuvers. Each maneuver 
will have its associated logic that will generate the 
control input to the model and thus provide the model 
reference states. 

For this thesis, only one such maneuver, a dive 
maneuver, was investigated. Logic for the dive maneuver is 


based on an application of bang-bang optimal control theory, 
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thereby yielding time optimal response. The methodology 
. here is to deflect stern planes up and the bow planes down 
to initiate a pitch rate (p) until the vehicle achieves some 
predetermined pitch angle (6). For what 1s considered 
reflexive, or emergency obstacle avoidance, a large angle is 
desired. Assuming that the eeene cab ile is directionally 
stable, some small stern plane angle must then be maintained 
to keep a constant pitch angle, dependent on speed, until 
such time when the control action should provide an opposite 
effect to come out of the dive and steady at a new depth. 
An example of this control action is shown in Figure 5.2. 

Given limits on control surface deflection and maximum 
pitch angle during the dive, this type of control action 
should provide an optimal response for a change in depth. 

With this control logic preprogrammed into an AUV, 
whenever the supervisory control system calls for a dive 
maneuver, the logic can provide the control input for the 
reference model and thus an optimal path can be created 
quickly; one that the controller can track and vehicle can 
follow. 

The logic for the dive maneuver is crude, however, this 
is a trade-off for ease in programming the algorithm used 
for the dive maneuver. When programming one of these 
reflexive maneuvers one must be cautious not to program a 


maneuver that is beyond the capability of the vehicle. 
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Figure 5.2 


ct 


ce 


Time Optimal Control Action for a 
Dive Maneuver Command Generation 
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A conceptual objective is to have many maneuver 
algorithms preprogrammed into a vehicle into a "bag" of 
maneuvers. This bag of maneuvers would be at the disposal 
of the supervisory control. This supervisory control would 
be the manager of the bag of maneuvers, as earlier indicated 
in Figure 1.3, and would receive its instructions from the 
on-board expert system oor, in the future, artificial 
intelligence. 

For the many types of standard and emergency situations 
required, collision or obstacle avoidance, a proper maneuver 
can be chosen and executed quickly and efficiently without 
excessive computational burdens that would otherwise lead to 


a tardy response. 


D. SYSTEM SIMULATION METHOD 
1. Dynamic Simulation Language (DSL) 

DSL is a Fortran based simulation language for 
aeeital Simulation of continuous’) systems. DSL uses a 
building -block approach to programming. peer ans can be 
very simple or they can become extremely complex when all 
the functions of DSL are utilized. The user can enter 
Fortran statements in any order and DSL can sort and solve 
these equations effectively. The user can also include 


fortran subroutines and use the expansive I/O facility of 


DSL. One other key feature is the integration routine 
capability. The user has the choice of nine integration 
methods; four fixed-step, two variable-step and three 
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variable-step, variable order methods. DSL was chosen 
primarily because it easily can solve differential equations 
and it contains many internal functions that normally would 
have to be programmed by the user. 
DSL has four phases of program execution; 
TRANSLATION 
COMPILATION 
SIMULATION 
GRAPHICS 
DSL translates all the DSL code into Fortran 
statements. Once the code is translated, it is then linked 
to the VS compiler and the code is compiled and stored as an 
executable file. Upon completion of the compilation phase, 
the simulation phase begins, and the system clock starts, 
and simulation continues until the system reaches its user 
specified finish time. The last phase of problem execution 
includes the graphic capability of DSL. Saved output data 
eae plotted or graphed using the graphics post-processor 
and the specific hardware supported. 
2. Problem Simulation 
As mentioned earlier, DSL uses a building-block 
approach to programming. The major blocks and general flow 
of program simulation are shown in Figures 5.3 and 5.4. To 
fully understand the simulation, and the controller design, 
and control action, a detailed breakdown and discussion is 


required. 
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CONSTANT 












HYDRODYNAMIC 
COEFFICIENTS 


INITIAL 


INITIALIZE 
MATRICES/ARRAYS 






CALCULATE 


M MATRIX 





CALCULATE CALCULATE 
A MATRIX B MATRIX 


CALCULATE CALCULATE BB = M-1B 


AA = M-1A 


READ IN 


GAINS Kk, Ko. kK, 





DERIVATIVE 


UMOD(xk) UMOD(k) 
( CONTINUED ) 


Figure 5.3 DSL Simulation Flow Diagram 
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( .CONTINUED ) 


UMOD(kK) 
UMOD(k) 


LINEAR MODEL 


Xm =. AAX,+ BBuU, 


CONTROL LAW 
Uy = k.* Xp * K 5 *X 


p m 


* ky) *Uy 


-” 


NON-LINEAR MODEL 
= M~lf(x,z,u) 





DYNAMIC 


ZORD, THEMG, ZPOS 


DIVE MANEUVER 
LOGIC 


CONTROL 
( FINTIM ) 
CHECK TIME 





STOP 


Figure 5.4 DSL Simulation Flow Diagram Continued 
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The first section of the simulation is the CONSTANT 
block. In this block all of the hydrodynamic coefficients 
and vehicle constants are read into the eEeaieart 

The second section is the INITIAL block. In this 
section, all of the calculations not part of the integration 
routines and those needed in establishing parameters and 
initial conditions are performed. This is also where all 
variables are initialized. The following calculations occur 
in this section: 

1. All matrices and arrays are initialized to zero. 


2. The length and weight fractions for a four term gauss 
quadrature are initialized. 


3. The breadth and height terms are read in. These terms 
will be used in the gauss quadrature integration for 
the crossflow drag terms. ; 

4. The thrust is then calculated for the propulsion 
model. 


5. The non-zero elements of mass matrix M are calculated. 
6. The square mass matrix M has rank of six is then 
inverted using the IMSL routine LINV2F. 


7. The non-zero elements of the A matrix are calculated. 
These elements are the coefficients of the first order 
terms in the Taylor series expansion about a specific 
operating point. 


8. The non-zero elements of the B matrix are then 
calculated. 


9. The next step is to multiply the inverse of the mass 
matrix to obtain the coefficients of the state 
equacion, (5.1). 


10. The last task for the initial section is to read in 
the computed feedforward and feedback gains from the 
program ETAT that are to be used in the autopilot 
CoOMmenol. Law. 
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The third block of the main program is called the 
DERIVATIVE section. Here, all the first order equations 
that must be integrated and solved are assembled. The 
DERIVATIVE section of this simulation is comprised of three 
major subsections, one for each major section in the 
simulation. 

1. Linear reference model providing command generation. 


2. Control law linking model and vehicle response to 
control surface actions. 


3. Nonlinear model for simulating vehicle response. 

The control vector u, is the input to the linear 
model, generated from the maneuver logic contained within 
the DYNAMIC section. This section will be discussed later. 

Once the control input is’ established, the 
derivative expressions of the linear reference model are 
formulated in terms of the matrices AA, and BB, - 

After the linear model derivatives are established, 
the model states x, and model inputs u, are passed to the 
Control Law. The Control Law (Equation (4.21)) represents, 
in software, the gains that would be incorporated in the 
vehicle. 

The input vector Up represents the inputs to the 
actual vehicle, in this case, defined by Equations (3.2). 

The derivatives of the vehicle state variables are 
formed as the last part of the DERIVATIVE section in 
preparation for numerical integration sustng een. see eeordes 


variable step Runge-Kutta technique. 
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The model states and inputs, as well as the vehicle 
states and inputs, are saved for graphical representation 
and data output. 

The fourth major block of the simulation is the 
DYNAMIC section. In the DYNAMIC section, the maneuver logic 
is programmed. This section is reserved for those 
computations that depend on time and are independent of the 
system responses. However, response dependent functions may 
also be included here as is the case with the establishment 
of the reference control commands generated by the maneuver 
ieee . | 

iit ec secelOnm Orem tne program is the @CONTROL 
section. Before the command or input is sent to the 
derivative section, the system time clock is checked, and if 
"finish time" (fintime) in the CONTROL section is reached 
the program stops. If not, the system increments itself one 
time step and continues with the simulation. 

Upon completion of the simulation a time history of 
all desired parameters and variables are saved in a data 
file. Plots and graphical output may then be generated. 

3. Procedure Used 

To perform a simulated run with a particular 
autopilot design and vehicle speed, an initial run with DSL 
was required to establish values for the AA, and_ BB 
matrices. These values were written on as output files 


(file Ftl0F001 for AA and file Ft0O9FO01 for BB). By 
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separate run, program ETAT was used to read AA and BB, and 
its own input file Ft0O5F001 for 9 and R and to provide 
values for control gains Kj, Kp and K3. The gain matrices, 
written on file Ft0O2F001 were then read by a final run using 
DSL for the controlled vehicle response simulation and 


results were provided on data file OUTP. 
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VI. RESULTS 


A. GENERAL 

This chapter describes the efforts and results of the 
design of a model following autopilot for an AUV. The 
controller designed is only a partial solution to the 
complete control over the six degrees of freedom of an AUV. 
However, the methodology developed in this study could be 
applied to design a full 30 state controller, 12 plant 
states, 12 model states, and 6 control states. The 
controller designed in this study is a 19 state controller 
using 12 plant states, 4 model states relating to the pitch 
plane, Wy, Gms 2m, G@_ and the three control inputs for this 
plane, port and starboard bow plane angle and stern plane 
angle. 

In Section D, the base-line Porro T ier is tested and the 
results show excellent depth control with excellent time 
mystery tracking. However, the control over pitch angle 
appeared loose and in Sections E and F attempts were made to 
fain tighter pitch control. 

A test of controller robustness is its ability to 
operate over a range of vehicle speeds and changing 
parameters. In Section G the controller was tested at 


speeds of 3, 12 and 30 ft/sec, approximately 1.8, 7 and 17 


of 


knots, respectively, while baseline runs were at 6 ft/sec 
(3. 5eknotsy 

Included in this chapter is a discussion of the gains 
used, how they were derived and the effects on the gains by 
varying the control weight matrix R and the control error 


weight matrix Q. 


B. RESULTS OF UNCONTROLLED MANEUVERING 

The first simulation runs that were made early in this 
study were to test the non-linear model. One maneuver that 
was first tested was a turning maneuver. Because of this 
AUV's particular geometry (low aspect wing vice body of 
revolution), some unique behavioral characteristics are 
displayed as shown in Figures 6.1 and 6.2, not common to 
other forms of underwater vehicles. Of the most significant 
is when a rudder command is given the vehicle rolls out of 
the turn. While this is not uncommon for vehicles without a 
sail area, it is uncommon for a vehicle with a cruciform 
type stern to dive on a turn while the vehicle rolls out. 
Although this vehicle's dynamics are not representative of 
those common to submersibles, the behavior has been verified 
experimentally. The purpose of selection was based purely 
on the availability and thoroughness to which the vehicle 
dynamics were modeled, and that program validation was 


easily accomplished from the data available. 
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C. DIVE PLANE MANEUVER AND PREDICTOR CONTROL 

Once the non-linear model was validated the controller 
design process then began. The first simulation of this 
process consisted of only predictor control, no feedback was 
incorporated. The inputs generated by the dive maneuver 
logic for stern and bow plane input to the linear model were 
also fed into the non-linear vehicle dynamics. 

This run, Figures 6.3 to 6.7, provided insight on the 
accuracy of the linearized version of the equations of 
joie On . Figure 6.7 shows excellent pitch correlation 
between the model and vehicle. 

Figures 6.4 and 6.5 both show that the vehicle never 
reaches its ordered depth of 100 ft. because the vehicle 
equations were linearized about a straight line trajectory 
at a constant speed, the linear model does not generate any 
speed loss and subsequently the AUV lags behind the linear 
model, a result that was clearly expected. The 
responsiveness of the vehicle is interesting considering the 
slow speed of 3.5 knots. 

Examination of the maneuver shows that a limit of about 
0.25 radians and 0.18 radians, respectively, was set by the 
command generation logic while the maximum pitch angle of 40 
degrees was reached and maintained after 16 seconds. Also, 
while the vehicle pitch angle is returned to a small value, 
when the control surfaces are returned to zero, a small 


residual pitch angle is left. This is a small point that 
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could be corrected by a refinement of the command generation 
ogaic . 

What is of interest is the magnitude of the final depth 
error generated by the difference between linear ead non- 
linear models. While the command generation logic drives 
the linear reference model to the targeted depth of 100 ft., 
the speed reduction in the AUV only provides a dive to 87 
feet--clearly indicating the need for corrective control 


action. 


D. EFFECT OF AUTOPILOT CONTROL--BASELINE CASE 

Figures 6.8 to 6.12 clearly show the difference the 
controller makes in attaining the ordered depth. This was 
the first simulation run using the full 19 state controller 
for control in the heave/pitch plane. Although excellent 
depth control was achieved, the pitch control was considered 
a little loose resulting mainly from the mismatch between 
model and AUV speed. Figure 6.12 shows the overshoot of the 
vehicle pitch during the maneuver. The overshoot of the 
pitch also is the primary reason for the vehicle attaining 
ordered depth much more rapidly than the model as shown in 
Figure 6.9. 

Other observations include, the majority of the control 
action comes from the stern plane which worked much harder 
than the bow planes. Figures 6.8 and 6.11 show the 
differences in control actions between the model and vehicle 


for stern and bow planes, respectively. 
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Because of the disparity in control efforts an attempt 
to equalize the relative amount of control actions and more 
closely following the model was made. As discussed in 
Chapter IV, the control weight matrix R (Table 1) was 
initially set up to penalize the rudder, rpm and buoyancy 
inputs, so that the primary control actions would be from 
the bow and stern planes as it would be for a dive maneuver. 
In this first run the weights were equal and the resulting 
control gains (Table 3) for the stern plane were much higher 
than for the bow planes. An attempt was made at sharing the 
control actions where weights of the R matrix were adjusted 
to penalize the stern plane and put more control effort in 
the bow planes. This resulted in a significant loss in the 
stern plane gain much less that one and only a very slight 
rise in the bow plane. gain. Although the resulting 
simulation showed more bow plane action it did not follow 
the model well and the stern plane became more active by the 
feedback action. This increased activity in the bow and 
stern planes resulted in very significant speed loss and 
excessive plane use was considered unacceptable. 

Upon further study of the vehicle and its dynamics, the 
reason for the inconsistency in poitrol actions is that the 
model maneuver treats bow and stern planes almost equally in 
their effect but in fact the force and moment generated by 
the stern plane is an order of magnitude more significant 
than that of the bow planes. Therefore, in future maneuver 
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TABLE 1 


TABLE OF 0 WEIGHTS --BASELINE 


Pitch Rate Error 


Q(5,5) 0.6 
Q( (5,14) -0.6 
Q(14,5) -0.6 
Q(14,14) 0.6 


Pitch Angle Errors 


Cid, a5) 2.5 
Q(11,16) -2.5 
Q(16,11) -2.5 
Q(16,16) 2.5 


Heave Rate Error 


Q(3,3) 1.0 
Q(3,13) =e 
ays) 2) -1.0 
Oise, 13) 1.0 


Heave Positional Error 


Q(9,9) 60.0 
Q(9,15) -60.0 
Ola) -60.0 
Q(15,15) 60.0 
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TABLE OF R WEIGHTS--BASELINE 


Rudder 


Starboard Bow Plane 


Port Bow Plane 


Stern Plane 


RPM 


Buoyancy 


TABLE 2 


Bia a) 


R(2,2) 


R(3,3) 


R(4,4) 


R(5,5) 


R(6,6) 
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0.0000000000E+00 
0.4954081951E-08 
0.0000000000E+00 
G.46449012840E-09 
0.2595669042E-05 
-0.4438610978E-04 
-0.5901765458E-07 
0.0000000000E+00 
0.7605860458E-04 
0.0000000000E+00 
0.72167 051 S56E>05 
0.4454871633E-01 
-0.6952859507E+00 
=0,7998 7295 s505E—02 
0.0000000000E+00 
0.76285088/78E-04 
0.0000000000E+00 
0.7236512897E-05 
0.4459061566E-01 
-0-6970854515E+00 
=0 39 /59326 5972-02 
0.0000000000E+00 
-0.6689594082E-05 
0.0000000000E+00 
-0.5682397825E-04 
-0.6106134440E-01 
0.5192655967E+01 
-0.100480542GE+01 
0.0000000000E+00 
-0.49904055922E-10 
0.0000000000E+00 
=-0 395557 702035E=)) 
=0. 159506 2760E=07 
0.4070361389E-06 
-0.4242388239E-07 
0.0000000000E+00 
0.8911601991E-10 
0.0000000000E+00 
0.1128610385E-10 
O.19297732elbave 
=0. 1207295075 E20>5 
-0.9768694409E-06 


4 
oooe oooooe oooooeo 


! it it 
ooooooe oOo © 


t 
So 


= 


S 


-0 


oooo°oeo oOo & 


_TABLE 3 


CONTROL GAINS 


sf CIS I DC OOUE ay 
-5019079092E-04 
.0000000000E+00 
»44922721241E-04 
~902507 927 5e-04G 
-1165944258E-05 


11 1G6a 576-05 
.4668891458E+00 
-0000000000E+00 
-69297937G40E+00 
-4676730097E+00 
-17496945879E-01 


~1117984364¢E-05 
-4682061530E+00 
.0000000000E+00 
-6947686167E+00 
-4689957818E+00 
AT SZIISTGZE= UT 


-9857068418E-05 
,oS6 59172647 Et01 
.0000000000E+00 
-5158026393E+01 
»3880347969E+01 
-1856029149E+00 


.7210798044E-12 


28997806499E-06 
OO0O00000000E+00 
4049075188E-06 
29122135626 -06 
126929 s02ZVES 07, 


1283218668E-11 
65227299G1E-06 
OO0D0000000E+00 
1210698470E-05 
6471117447E-06 
6867469453E-08 
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-0 
-0 


-0 
= 
-0 


-0 
=0 
-0 


=0 


-0 


-0 


-0 


-0 


W251 5687 S68E=05 
-2760571526E-08 
121 9Ce/ Fool eu 
.0000000000E+00 
-2764935447E-05 
-1165974829E-05 


-4422518793E-01 
-4168961983E-04 
-4510255945E-01 
-0000000000E+00 
-4530064631E-01 
»1747022491E-01 


-44926664311E-01 
-4182848952E-04 
-4518982016E-01 
.0000000000E+00 
ncaa See ane ne 


-6050596839E-01 
-42178290356E-03 
-2348149885E+00 
-0000000000E+00 
=O, 
16390106 se. 00 


-1382443897E-07 
.2934508269E-10 
-2184067568E-07 
.0000000000E+00 
»ClLID( Si cooes Os 
»1269214802E-07 


1916258257 E-016 
-22 08999691 E=10 
a0. 
.0O000000000E+00 
.1220747899E-06 
-6880516105E-08 


1753074334E-01 


2363129821E+00 


LZAV S875 2Zeb=06 


RUDDER 


PORT 
BOW PLANE 


STBD 
BOW PLANE 


STERN 
PLANE 


RPM 


BUOYANCY 


model generation it should be noted that bow plane forces 
and moments are more subtle and should be used for fine 
depth control rather than for major depth excursions. 
Considering the speed mismatch, the overall control was 


considered acceptable. 


E. EFFECT OF TIGHTER PITCH CONTROL WEIGHTING 

Due to the unique dynamics of the vehicle it was decided 
to leave the control weights the same in the R matrix, as it 
was in the first run, with the understanding that the model 
maneuver algorithm perhaps wasn't as well suited for this 
vehicle as it could have been. 

With the R matrix fixed, with equal weights on the bow 
and stern planes, it was decided to adjust the weights in 
the © matrix to try to gain better control over the pitch, 
and to increase the pitch error gains. The weights that 
were adjusted were those that related pitch errors, elements 
aly 11), O(11,16), Q(16,11), @(16,16). | 

When these elements were increased by a factor of 1000 
the pitch error gains increased and a tighter control over 
pitch was achieved as shown in Figures 6.13 to 6.17. 

Comparing Figures 6.7 and 6.17 shows the tighter control 
gained over the pitch. With the tighter control gained in 
pitch a slight degradation in depth control was observed. 
Figures 6.14 and 6.15 show a small overshoot in ordered 
depth for the vehicle indicating the loosening of the depth 


control modes. 


FeAl 


a[buy euetTd urze4sg Hbutzyhtem TorAQuOD youTtTd AeqYybTL ET*9 eANbty 


(OAS) ANIL 
002d OUT O9T OrT Qcl OOT OV 09 OF 0c 


¢ + 6 4 4 
8 6 8 8 ' 
‘ e 8 8 
e 6 6 6 8 
8 e 6 e 6 
6 e 6 6 6 
6 6 6 8 4 
6 6 * 8 
6 6 8 0 8 
e 8 * 8 
8 8 6 6 
eee ree eeesece doce rcccccccsedoccsccccccccctcccccsccscscesecvccccscccceloncccsccccccc cnc vcccsccesetccveveccccccedccccccncccccedsncccsccsccces 

6 % 8 
% 8 
6 8 
6 4 
8 6 8 8 
6 + 

e 

* 


CUTS SS SCE HSS HSOSHHHE ESS SOHO EHESHH EH EATS HFEF EH HOHE ETOH HF FSET HF BO LEM EHO BHF FFB eww AETeeheaeseterieeqQesesevereence 


TIOHAA . |) 3 7 | 
(NAOH : 





a eee ®eeeee@evewee é ®ee ®@hoeveeeesn ® a eee @®@eeeeeeeee 4 eeteeeeeeeveee é Seeeneeeeeeveee a S@e-fe eeeeeeweje a ®eeeceeveen @evenvs 
¢ 6 + 
e 6 a 
8 6 ' 
8 + 4 6 
8 6 6 ¢ 
8 8 ¢ 4 
e e 6 
8 e 4 6 
9 % 6 
6 6 e 6 
6 8 6 8 6 
6 6 ‘ 6 
—_—w Na =FJeF®reeeeee-f= qeeeoetcwaevnveoe ee 
4 6 6 
6 8 8 6 
6 6 6 
8 6 a e 
6 6 6 6 6 
e 8 ¢ 6 
¢ 4 ‘ 6 
¢ e 8 % 
6 a 8 e 
6 6 6 6 8 
6 8 a ‘ 
eer eereeeereeee 4 ooo mm wine eee ee enna ia nim =) © = ee Riel mma Riera ~ © >: 4 «\n)e\e ol e.eiaia =) sole Nbie cinemas icc ibet sins 0 6/6 asics c\stn mwas ceere reel wiv ewcwsccemeic I Sais ss evzrereeee 
6 8 ¢ 8 
6 6 6 ‘ 
6 6 6 6 
4 8 ¢ ‘ 
¢ 6 + ‘ 
‘ 6 6 ¢ 
6 6 8 4 
6 6 % ‘ 
6 e 6 8 
8 8 6 ‘ 
6 6 6 ‘ 
ee were rzreereeee qe er eee eeeeeeegreZreerzreareeeFegZFeeFeeaeeeFeFqgeetFaeeeeeeeeYsF®uaneeeneerteaeeqgeeeneeeeaseeeeqTwneeeeeeeees ee gether tee rt oven qgeseeeoecheantse 
‘ 6 6 
6 6 6 4 
8 8 8 4 
6 8 6 8 
6 8 8 
6 6 6 6 
6 6 6 
6 ° e 8 ieee 
6 6 6 6 
8 6 6 ‘ 
8 6 6 ‘ 
eeeesenae eveeeteeeee Jo cwerrcecewwedececwcceceroedececcecccccesdoneceecevcceodsweeccsceseratowscceressecatovecesccccccedecscnccnsscsetocccns ee ee 
8 8 6 ‘ 
8 6 6 6 
8 6 6 6 
6 8 6 6 
¢ . 6 6 
6 8 6 6 
4 6 6 6 
¢ 6 e 4 
‘ 4 a 6 
8 * 6 ' 
6 6 8 


1 O= "ac OS amenO ent 0 
ANW Td NdeLts 


0°O 


TO 


c 0 


(SNVIGVY) FISNV 


£0 


v0 


72 


STTFJOId SATC Hutjzyhtam Tor}uo0D yo utd AaAybhty 


(La) SOdX 


PI°9 eanbtTy 


O0cT OOOT 008 009 OOP Q0¢ 





' 8 8 8 
a 6 t 8 
s 4 4 8 
8 4 5 8 
4 4 4 8 
e 4 8 8 
t a 8 8 
6 4 + 8 
a 4 8 + 
4 4 8 8 
4 4 6 
8 4 8 8 
t 4 8 8 
4 4 8 t 
L} 4 8 4 
8 oS SS eS FE FE FO OU VU ow ; OE OP NR CNR wee CAE Sy SO SO Pal oa al a Oe OM OM MM Moh “oh Ml ae ae ee ae ae Tae eee eee antl Aad 9 2 2 2 2 2 
e ‘ ‘ pe Or a Oe ee ree a koe 
‘ ‘ a ‘ ~ 
8 a 8 8 
4 4 8 
4 4 8 8 
LY 4 6 4 
t + . 8 
4 4 4 8 
8 4 8 6 
6 4 4 4 
4 4 4 4 
‘4 8 4 4 
4 4 4 4 
4 a s 4 
4 4 4 4 
eeeeeeeeoseeeeeeeeetevecestsceeeseeeeseeeeeeeeeeee boeeeeeesteeeseeen ee eases aebaseeeeseereese Geese ee es ete nee naeeeeseseseeeeeeeen 
4 4 8 4 
4 a 4 4 
4 4 4 4 
4 ‘ 8 4 
4 4 4 4 
4 4 ® 4 
a 4 4 4 
8 4 4 
8 4 8 8 
4 4 4 4 
8 4 4 4 
8 4 8 4 
4 4 4 4 
* 4a 4 4 
a 4 8 # 
4 4 4 4 
eeenneeeeeoeeeeneneeeen et ee ee ee ee ee 
4 6 8 4 
4a 4 8 4 
4 8 4 4 
4a 4 a 4 
4 a a 4 
8 a 8 4 
a a 6 4 
‘ 5 4 ‘ 
4 # 8 4 
8 4 8 
8 4 8 
o 4 a 
4 4 4 
6 6 a 
8 4 4 
} { @ee@eeeeeeeaoeeweeveeeeeeeeeeene ; ee@e@eeeeeoeeeeeeeeeeeeeeen 
4 8 * 
6 8 4 
8 * # 
F 4 4 4 
5 4 8 
8 8 s 
4 4 8 
e 4 ‘ 
4 4 4 
6 8 4 
4 4 4 
e 4 4 
4 4 4 
, 4 8 
8 4 8 
& #oeeneeaner aan Ge Geneve nsv vlaer nee eeannneneeenezeeten 
4 8 s ‘ 
4a 4 4 4 
4 8 a ‘ 
4 8 e 8 
8 8 s 4 
‘ 8 eae 8 
4 8 & a 
a 8 + 4 
4 8 a 8 
4 4 ‘ 4 
e 6 # 4 
4 s * 4 
8 8 # 4 
4 8 4 4 
4 4 4a 4 
4 5 4 


ew eesee@eseeeeeeeeeeev eee 


ee nea eee oe 
a 


Ocal 


(Ld) SOdZ 


13 


SoUTL SA Yy4deq buT4YybtamM Torz,uo0|D YORTd A9QYbty 


00¢ 


oF OE oe ow ow o@® o' 


eeeeeeeeeeeee 





© 
oD 
| 
© 
© 
Game} 


OVT 


4 ‘ ‘ 
9 6 % 
8 + ¢ 
8 % 8 
8 8 8 
8 6 8 
8 6 8 
8 8 8 
a 8 4 
8 8 8 
4 8 8 
‘ 9 ‘ 
4 8 4 
9 4 ‘ 
4 ' 4 

er oe a el Ee ee a a Se ae) Sear, 


ereeeove@eneeeeeeoqreeeceenenoeeeee@ 


cet @eeer eer ete eeeecern eee e®eeaeaohtaateaneaeeanevte 


A TOTHAA 
QNHOT 


’ 
5 
4 
' 
8 
8 
9 
8 
8 
8 
9 
8 
¢ 
8 
8 
¢ 
8 
8 
s 


eeeceeceneveceneecnenert(eet® eer ere eec ee ncet@we er eee seeeeceaoen Foe eeenr ee eekxe erence eterceeto2t 2 eer eeeee 


Sem Oem Oem Oat Cat) “em * 


Qcl 


eeeeneceneene2enananece®aarneaans alialatale alalcialelaletatalalaletelatalstatatatatalatatetatalatelslatalatetelalsla tl aletatatetatatatatetatatetatta @eeeonee et 2 @Gere 2 
° 


(OAS) ANIL 


@eeeneeeeaeceers 


OOT 


eeenrezpeeeaneeece 


ee@eeeeererececeeceee ebheaeeecntercneerveaaneve ctdvnceececececeneant eect anveteeeaeenenne2e2enec2eececetch eae aeeteaneneeaeaeece 


@eeeeeeeoeveso 


me@eeeeoeveeee2 


e@eeenoeeeoestovene 


O08 


Se eeeeeeeeeeeewoeheeeeecan sean se teense ttoeeecesecetaonenennveteoncenecenentenenrecaetéebataoaenetoaaneteteoaceon 


*#Geenerneneeeeneneacoer«a 


oo @ 
_— se ee = a 


J 


eseensvnveeee ee Be 


eeeoevee2 722 ® 8 @ 


eeeeneceneenececenneoeteeecen ane eeenecaevae tw eto eeceeneneceenecenecetoneeatoaanneenannecececethtaneeananeeaneananaee 


a) 
© 


eoeaevveeveevee eerte 


© 
i 


meeeeenennxeneancas 


@eeeterrzreteaeneeceneceteeneneneceeceenen eas Hex eerezc eer er etzeceetowcearxeceeceenenenet ec®aerhtaasne@nznaerteaneenen 


ST°9 eanbTy 


© 
A 


rPewmeeeeveeeres 


eseeenrveeneeee ees 


@eeetenenennneeeecaoteecenenenn een e te e2eaece tte eeana 
' 
— 


74 


002 OUT O9T OVT Ocal OOT 





aTbuy sueTad Mog buTqubtom TorqUCD YOaTd AeqzUubTL 91°9 sanbtg 


(ONS) AWILL 


O8 09 OF Oc 0 


\ 
: : ; i ' —) 
: : she 
3 ? 3 3 3 
ee ee ae ear Poccccccccc nce docncccccncecedacccccvncrecsedaccnnascencentovcnncnarcaccdcccerncccccerdoncccccrcnccnbnccencacsceccdecvacacccance © 
: Fe) 
, 77 oo 
‘ | { @ 
‘ ‘ ‘ ‘ f au 
Riga ein reels aS cic es5 a we reis oc ease re ice econ ee ee sciakene eases q BID laie ib 'siw.e Siac g 3h sie e deg Oe ee ae o~< 
THOGONW |: 3 3 NY N rg 
ATOTHAA : | _ | tC 
(NAD | : | > 
Powrcreccocece Docccccccccces Dawe rccrcccece Seereccoctuves Deceereccccees Deceepoceccece Becece Ne ewiss © Ps, 
: : 3 | adi m ty 
3 3 ti > 
3 3 3 : i _- 
a ae — 7 Se oe ee a | — 4 
aa, (3 — 
3 3 3 7a Cy 
3 3 , 7 ‘ = 
wee re en eee. , Maeve Ge, he ibhscvnadet gees lad ee 
_ i > 
3 3 oer = 
ies > 
Ran). SN ac Re Nas I ee ara nT MUR ent oy ec cL eee Ee nies 2, 
3 3 3 : nm 
ee a a a a oe ee te it 182 dls 2 ae ee ea ae © 
oo 


uD 


002 OoT O9T OrT Ocl 





eTbuw youtd butyyhbtemM TorA,uoOD yo Rtd Aaqyhty 


(ONS) ANIL 
OOT 


O8 09 OF 


LT°9 eanbty 


© 
OW 


‘ 8 4 4 ’ 8 ’ 
e ‘ ‘ 4 e a a 8 ‘ 
‘ 8 ‘ e e e e 8 e 
+ 6 e e e e + 6 e 
e 8 * 6 ¢ ‘ 8 ® ¢ 
e a 4 e a e 4 e e 
¢ + 4 * 6 8 8 e e 
e a e e e ¢ a ¢ e 
e 8 e e ¢ e e e 
e 8 ‘ e e e a 8 e 
¢ 8 ¢ a ¢ ‘ 8 4 e 
eocccetraeccccetdecccvevccecsccetccsvccccceteocetccesccctcccecetccccecccccscce Bowcceccvecacccedseveesccceccvece evccece oe ercecetececescetbsccccetcsccceoen 
eo 4 4 ° 4 4 ; 4 4 er oe ee ee | 4 ° ° 4 e 
e 8 e e e e ‘ e e 
e 8 e 0 e e ‘ 8 e 
e e a e e a 8 e e 
6 8 e e e e ¢ e e 
e e 6 e 9 ¢ 6 e e 
‘ ‘ 4 e e e t e e 
e e e e ¢ e e 
4 ¢ a + i e ¢ 
e e 8 ¢ a e e 
4 e e e e e e 
e e e 8 ‘ 8 e 
Ce 
¢ e ‘ 4 ® ‘ 
e ¢ ¢ ¢ * 
e e e e e Q 
e e e e e e 
e e e e e 
e 0 e e e 8 e 
e e e e * e 
e e e e e ‘ e 
e e e e e e 
e e e e 0 e 
e e e e e e e 
decccecesecesse deere cccccrece Peoecccccccccece Secevccccereces Seceaccrevcsece deco cccccrtface deccvcch crcccce 
¢ ¢ e é e 
e e e e e e 
e ¢ e e e e 
‘ e 4 e e e 
e e e e e e 
° e Q e e e tt--~ 
¢ e ¢ ¢ ¢ \ 
‘ ‘ ‘ ‘ ‘ ¢ i o6 
8 a ® e ‘ 8 ® e \ 
8 , Q e e e 8 i e 
’ ® ‘ 8 a e i e \ 
a a e ‘ ‘ 8 e 9 e 
fe we ce ewes ce gece cc cw ees cce ge cece ccc cece geerccc cece ce cence ccc eee ccc ge cares ace acne geresaraescsecgecceesceseccccyccccccstacatecqeccoce Ve cccece 
8 ¢ 6 8 Q ‘ ‘ ¢ f ‘ 
‘ e 6 8 8 ¢ e ] 
8 e ‘ e e ‘ a e i e 
8 e e e 6 8 ‘ ° e \ 
4 e e 6 a 6 * a ‘ e 
’ ‘ ‘ 6 ¢ ‘ ‘ ¢ ‘ \ 
e e e e e ‘ 4 e f e 
e ¢ e e a 8 e e / e 4 
e e e e ‘ ° e e e 
e e e e ‘ ‘ e e t e 
e e e e e e e e e 
e@eeeeeneeeeeevetoseestseneeeee eee 4 @eeeneeeseescveseeeedtceaoeveeene eevee Ree é PeewreeteteeeeceetgeoeoeteaeaeteancseeeeeetetFtFaeeoeeveewaeaaeaecaaeeaane 4 eeeceeaceeoneweaeceaeteaote eee mfunacaceetataaneteaeve ee 
re : ¢ 4 3 ‘3 ° ° ded ae oi ‘ ; 1 ; 
e e e e + e ‘ ‘ i ‘ 
e e e e e e e e ‘ 
e e e e a ‘ ¢ e t ‘ 
e e e e e Q e 8 e 
® e e e e e a e t ‘ 
e e e e e 8 e ‘ e 
e e e e e e e a t e 
e e e a e e e e 
e ¢ e 4 e 8 * { 1 
e e e e e e a e 
e e e 9 e e e e J ‘ 
es@aeeeeeeaeeereaee yt PPR HH eORHF EF ee erresveoesereresaeeeqgqeeeeteovoeePeereqeseeeeereevetereqesreeeeveBeeeeeqesceeaneaeteaeeeenqgeateneneeeevrenraqeses @eeenwwanqeweeetenotrtaave ee 
¢ 9 ¢ e ¢ + 8 e j 8 
0 e e e e e e 8 ‘ 
4 9 ¢ e e e + ¢ i ‘ 
a % ¢ e @ + + e ¢ 
¢ 4 e e ¢ e e ¢ f a 
¢ ¢ e e ¢ * i 4 i} 
¢ ¢ e e ‘ t a a i ¢ 
9 6 a ® + 8 e * ¢ 
e e ¢ ¢ * ¢ + i ‘ 
e e 4 e e e e 8 ‘ 
a e ¢ ® 8 ‘ 8 ‘ i 8 
eeeeeeeeeeeee Utelereielole el elslelele else aes eee 6 ales sl tiale a ie 6)s\e elelaliate tialelaletelslotetalelatetertietelsletstetateloletateleriicicleicielcic etersieicieisdlelea cleleise csicrctercric . ee ee a ee ee ee ee ee 
e 6 a e + t t a { 
e e e e e 8 a 8 
e e e e e e e i 
e e e e e e ¢ a 
e e e ‘ e e ‘ + 4 
4 ¢ 8 — am Um ot 
& t e e 
e e e e e e e 
e e a e e 4 e 
e e 0 e e a e 
e e e e e a ‘ 
a + ¢ * ¢ ‘ 


Cr- cc- Gc9- GL- 


co- 


cE- 
(SAAUDAC) ATONV HOLId 


cI- 


76 


F. FURTHER PITCH CONTROL WEIGHTING 

. The Q matrix pitch elements were further increased by a 
factor of 10 to observe the correlation between depth and 
pitch control. Again Figures 6.18 to 6.22 show a sluggish 
response in depth control while gaining a much tighter 
control over pitch. However, in this case the command for 
the bow planes have exceeded their physical limits and are 
commanded to an unreasonable amount as shown in Figure 6.21. 

As the linear controller can command a control action 
greater than the physical limits of the vehicle, when poor 
weights are selected, logic was added to the DSL code to 
limit the plane action to plus or minus .6 radians on the 
bow and stern planes. 

Although the increased weights in the Q matrix gave a 
better pitch response, its effects on tracking control were 
undesirable. For this reason, and for all subsequent 
numerical experiments, it was decided to Ge the gains 
originally calculated in the baseline run and the original Q 


matrix weights. 


G. EFFECT OF SPEED MISMATCH MODEL/VEHICLE 

The major issue of control robustness relates to the 
seriousness of speed mismatch between model and AUV. So the 
next task was to test this controller over a range of 
vehicle speeds, 3, 12 and 30 feet/sec. 

Using the controller and model based on a speed of 6 


ft/sec, the simulation was run using a vehicle speed of 12 
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ft/sec. Pigimees (6.23 “tO 6.27 Zou VeEGvemgood, thackind 
ability even though the vehicle was going twice the speed. 
Figure 6.24 shows that the vehicle went twice as far as the 
model to reach the same depth, due primarily to the vehicle 
speed being double that of the model. Figure 6.27 shows the 
compensation in pitch angle to achieve desired depth. ie 
the controller was tighter in pitch it would have followed 
closer in this figure but in Figure 6.25 the accurate 
trajectory tracking would be lost. Again this goes back to 
the type of control needed and adjusting of the weights in 
the © and R matrix to generate satisfactory control gains. 

The next test of the controller was an attempt to run 
the vehicle at a speed of 3 ft/sec which is very slow and 
yet try to use a model speed of 6 ft/sec. The primary 
motivation was to see if one set of gains and one model 
could be used for all maneuvers, rather than recalculating 
gains every time the vehicle changed speeds; a test of 
robustness in the controller. When the vehicle was operated 
at 3 ft/sec the vehicle started out lagging the model and 
then control errors grew while the controller commanded more 
and more action. But, since the vehicle was much slower 
than the model ordered, depth and path following could not 
be achieved. 

To alleviate this problem, gains were recalculated and 
the model was run at 3 ft/sec (Figures 6.28 to 6.32) when 


excellent tracking was restored. However, at the slower 
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speed (only 1.75 knots) the dive maneuver algorithm used was 
not. sufficient to maintain pitch during a diving trajectory. 
The buoyant moment overcame the hydrodynamic moment from 
control surfaces and ordered depth was not achieved. This 
behavior, however, is not characteristic of the controller 
but rather the maneuver logic, and as far as the controller 
is concerned it was able to follow the model rather nicely. 
Since the methodology here was to design a controller 
that was robust enough to handle a wide variety of reflexive 
type maneuvers over a range of speeds it is most likely that 
the vehicle will be traveling at much greater speeds when 
these maneuvers are executed. For this reason, another 
Simulation run was made. Again the control weights and 
gains used were as per the baseline case of 6 ft/sec. The 
model was also at 6 ft/sec and this time the vehicle was at 
30 ft/sec. Figures 6.33 to 6.37 show once again excellent 
tracking control, and like the 12 ft/sec case tight pitch 
control was eased in favor of accurate depth and trajectory 
control, which is desirable not to have the vehicle 
violently pitching during a maneuver which may result in 


vehicle equipment damage. 


H. EIGENVALUES--LINEAR MODEL 
The following presents a table of the eigenvalues of the 
baseline model at 6 ft/sec forward speed together with the 


closed loop eigenvalues found using the baseline weights. 
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Eigenvalues 
1 


2 


10 
11 
12 
13 
14 
15 
16 
ae 
18 


19 


OPEN 


TABLE 4 


LOOP EIGENVALUES 


Real Part 


-0.1600E-02 
=O. [50CE—-02 
-0.1700E-02 
-0.1800E-02 
-~0.2100E-02 
-0.1000E-03 
-0.1663E+01 
-0.657 9E.00 
=0.9553E1T00 
-0.1122E+00 
-0.1122E+00 
-0.3909E+00 
=O. 1603E—02 
=0 95538700 
=O SSOSETOG 
=0. 1425808 
-0.3000E-03 
-0.4000E-03 


=O, 50008>Us 
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Imaginary Part 


0. 0000E+00 
Q0.Q0000E+00 
Q0.Q000E+00 
0.0000E+00 
0.0000E+00 
0. 0000E+00 
0. 0O0O00E+00 
0.QO000E+00 
0.0000E+00 
Q0.7003E-02 
-0.7003E-02 
0.0000E+00 
0. 0000E+00 
0. 0000E+00 
Q0.0000E+00 
Q0.Q000E+00 
0.Q0Q000E+00 
Q0.Q0000E+00 


0. 0OOO00E+00 


Eigenvalues 
ne 


2 


10 
a 
a2 
a3 
14 
LS 
16 
a) 
18 


ao 


TABLE 5 


CLOSED LOOP EIGENVALUES 


Real Part 


-0.1600E-02 
-0.1500E-02 
a0. 17 OOH=O 2 
-0.2100E-02 
=@2)1663E702 
-0.9888E+00 
-0.3456E+00 
-0.3454E+00 
-0.6579E+00 
-0.4868E+00 
=0, 95538200 
SO 2 OO 
=Oe LI 22 E00 
-0.3908E+00 
=-0.1423E-01 
=O. .000K=0s 
-0.3000E-03 
-0.4000E-03 


-0.5000E-03 


LOL 


Imaginary Part 


0.0000E+00 
0.0000E+00 
0. Q000E+00 
0.0000E+00 
0.0000E+00 
0.0000E+00 
0.4402E+00 
-0.4402E+00 
0.0000E+00 
0.Q000E+00 
0.Q000E+00 
0.7003E-02 
Oe AU os — U2 
0.0000E+00 
Q0.0000E+00 
0.0000E+00 
0.Q0000E+00 
0.0000E+00 


0.0000E+00 


VII. SUMMARY, CONCLUSIONS, AND RECOMMENDATIONS 


A. SUMMARY 
This thesis presents a study of Model Reference Controls 
for an Autonomous Underwater Vehicle. The approach to the 


design and testing of a model following autopilot included: 


1. 


B. 


Selection of a suitable submersible was selected, one 
that displayed many attributes for potential AUV 
missions. One in which all the hydrodynamic 
characteristics were well studied and data were 
obtainable. 

The tailoring of the existing equations of motion to 
gain control over all six degrees of freedom. 


The development of a linearized model and programming 
the linearized and non-linear models for simulation 
using Dynamic Simulation Language (DSL). 


The development of a 19 state controller for dive 
plane maneuvers. Maneuvers that could be termed 
reflexive. 


The development of logic for a command generation 
system for a dive maneuver. 


Observation of the effects on the control gains by 
varying the weights in the minimizing function J. 


The testing of the command generation logic and the 
controller over a wide range of speeds using only one 
set of calculated gains based on one speed of 6 
ft/sec. 


CONCLUSIONS 


In this study, a methodology was developed to the design 


of a model following autopilot that could be used in an 


Autonomous Underwater Vehicle. A 19 state controller was 


designed for automatic control of maneuvers in the dive 
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plane. This controller displayed excellent trajectory 
following characteristics and exhibited a high degree of 
robustness over a five to one speed range. 

The model following autopilot was designed to follow 
trajectories generated from a preprogrammed maneuver 
algorithn. This maneuver logic proved to be workable and 
could easily be developed for a wide variety of maneuvers to 
be stored on-board in a computer. 

In this study maneuver logic was created for one such 
maneuver, a dive maneuver, and was followed by the designed 
autopilot. The algorithm used for the dive maneuver was 
erude but sufficiently proved that the design methods are 
sound. 

Some difficulties in perfect trajectory following occur 
because of speed mismatch between model and vehicle, and an 
improvement in modelling speed loss during maneuvers would 


be worthwhile. 


C. RECOMMENDATIONS 
Because the concept of Autonomous Underwater Vehicles is 
fresh and significant progress has been made in the 
computational abilities of modern computers, a wide 
diversification of technological avenues’ need 26 be 
explored. Specific to this study the following 
recommendations are presented. 
1. An implementation of the full 30 state dynamically 


coupled controller in an AUV should be the ultimate 
goal of this project. In particular, the influence 
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for forward speed changes should be reflected in the 
maneuver command generation model for greater control 
accuracy. 


Parallel efforts should be carried forward with the 
development of many maneuver algorithms that could be 
stored in the AUV's "bag" of maneuvers. 


Although this controller was designed specifically for 
the control of an AUV with an unusual geometry, it can 
and should be tested on underwater vehicles with other 
geometries. 
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APPENDIX A 


SIX-DEGREE-OF-FREEDOM EQUATIONS OF MOTION 
AND EULER ANGLE RATES 


SURGE EQUATION OF MOTION 


m[u - vr + wq - xg(q? + r¥) + ye(pq - r) + Z2g(pr + q)] 
Zoe 
» ° ' ' ' 
1 5 LIX ut Xwq WO + Xyp VP + Xyy VE 
q - J q 
is ud (Xg gs Oe + Xqéb/2 ee + Xq db/2 Shs 
aE ae ur 6; ] (A-1) 
+ by [X yy Vier wel Xe Gr UV 6, 
+ uW(Xwss Ss + XwSb/2 Sbs 
Pee os) 
wob/2 bp 
2 ' 2 ' 52 ' 52 
+ u“(X sss Ss + XShdb/2 bp + Xdbdbs2 °bs 
' 2 
+ Xeror Sx) ] 
- (W-B) sin 9+ 5 east ug 5, e(n) 


’ ' 2 
+ 5 2 [Xwésn ivoew eres ene oa] = (1) 


(eee 
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SWAY EQUATION OF MOTION 


miv + ur ~ wp + Xc (pq + Yr) - Yq (p? + r?) 


+ Ze(doee D) J 


= 5 £*(¥5 p + Yr © + Yong PG + Ygr ar] 


z ° g g 
+ 5 WP Yy v a. Yo up + YY} ur + Yvq Vq 
t g 
+ Ywp wp + Ywr wr] (A~2) 
g a a 
+ = g2[Yy uv + Yow vw + Yop u2 6] 
x 
: nose 5 
“_ oa [Coy h(x) (v+xr) 
a 
tail 


+ Cp b(x) (w-xq) 2] - dx 
Cr 


+ (W-B) cos © sin 6 
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HEAVE EQUATION OF MOTION 


Newest Ug + Vp + Xq (pr = Gl ae Veudr Fp) 


Seccres = da) 
aa. 24 q+ Zpp p- + Zor Pr + Zryr r2] 


+ s 93 [ Ze w t+ Zq ug + Zyp Vp + Zyy Vr] 
- . (22 uw zee (A-3) 
+ u2(Zés bs + Z6p/2 Sbs + Zgb/2 “ppl 
o , nose 5 
es =~ [Cpy Cy axe) 
“ead 


+ Chg b(xX) (w-xq) 7] ote) ax 
Ee 


+ (W-B) cos 9% cos ¢ 


g Zqn uq «&n) 


4- 
No 


+ 5 2 (Zyn uw + Zssn u2 6s] e€(n) 
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ROLL EQUATION OF MOTION 


ly p tea hoe Ty) qr + Iyy (Pr sali)! ae Iyz(q? = Ee) 
~ Iy-(pqtr) + mMlyc(W = Ug + VPI 2647 2) ues ewe 
0 Le i t ? 
5 Kp p+ Ky © + Kyog PG + Kgr ar] 
p ne ' ' ' 
7 24rre- es 
ao [Ky Vv + Kp Up a Ky UES Kyq Vq (A-4) 
+ Kwp wp + Ky; wr] 
oa . : ? 
+ 2k [Ky uv + Kyy vw + u* (Ks p72 S bp as Ksb/2 Shs) ] 
+ (yoGW - ypB) cos § cos > - (Z@W - ZpB) cos % sin? 


+ £24 Kpn up € (n) 


ese ae 
+ 5 £~ u Kprop 
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PITCH EQUATION OF MOTION 


ly Gewe( ly = 15) pr = Tyy (ar =) oot Iyz (Pq =r) 


Te Ty (p?-r?) - m[X¢(W - uq + vp) - Z2@(u - vr + wq] 


t e 4 ' 
ome iad + Mo, pe + Moe pr + My; ©] 


0 ; a t t 
+ + S4(My wot Mg uq + Myp vp + Myr vr] 
a5 23M, uw + Muy v2 
“ u2 (Mg os + Mob/2 Sbp + Msgp/2 She) J 

X 
nose z 

+ = [Cpy h(x) (v+xr) 
ee 
tail 


+ Cp, b(x) (w-xa) 2] ote x dx 
Cc 


- (XGW - xXpB) cos @ cos ¢ - (ZGW - ZpB) sin 6 


+ 
Nolo 


Ps, a etn) 


93 [Myr ee ery u2 O=) e(n) 


+ 
No 
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YAW EQUATION OF MOTION 


Iz r+ (ly - 1y)Bd > Seye-oa- ee) 
te Ty (ar-p) + m[Xcg¢(v + ur - Wp) - Ye(u - vr + wq)] 


2° Ns p+ Ny © + Nog Pd + Nor QF] 


| 
Nf 


+ 
Nir) 


7 ' 
LING v + Np up + Ny ur + Nyg vq 


+ Nywp WP + Nyr wr] 


a 8 
Mt Ny uv + Nyy vw + Ney ut 57] 


+ 
Nofo 


x 
jie nee meg toca) 


Noto 


Xrail 
(v+xr ) ara 


+ Cpz b(x) (w-xq) 2] Une 


+ (XGW - xXpB) cos 8 sing + (yq@W - ypB) sin 8 


DoS oO 


LO 


Euler Angle Rates 


‘ = p+dqsin $¢ tan 0+ r cos $6 tan @ 
Q = qcos ¢- r sin 9 
y= sin ty £98 o 
4 cos 5 cos J 
Inertial Position Rates 
“9 = Meo = U cos yj cos 6 
+ v({cos w sin @ sin $¢ - sin yj cos 
+ w{cos wy sin @ cos $+ sin y sin 
va come sin y cos @ 
+ v{sin wp sin 6 sin 6 + cos yj cos 
+ w([sin wy sin 6 cos » - cos jw sin 
a = We9 - usin 6 


+ v cos @ sin jy 


+ w cos §cos ¢ 
Crossflow Velocity 


Eeecor= Uny © xr)? + (w - xq)2]1/2 
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TITLE AUTONOMOUS UNDERWATER VEHICLE oe SIMULATION 

D COMMON /BLOCK1 / MMINV (6 6 AA(12,12), BB(6,6) 

D COMMON/BLOCK2/ B 6 58), A(12 12), UMOD(6) ,GRK(6,21) 

D COMMON/BLOCK3/ F 2), Fala): UCE(4) 

D COMMON/BLOCK4/ G (4 Gk4(4) ,BR(4 dH H(4) 

D COMMON/BLOCKS/ XDOT(12),XDOTX(12), XDOTU(6) 

FIXED N,IA,IDGT,IER,LAST,J,K,M,JJ,KK,I 

INTEGER 

ARRAY WKAREA(54), X(12) 

* 

CONST 

z LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 

CONST XPP = SOO = ,XRR = _XPR = 
XUDOT= /XWO = UXVP = .XVR = 
KODS= /XODB= /XRDR= XVV = 

/XVDR= /XWDS= /XWDB= 

XDSDS= _XDBDB= , XDRDR= ,XODSN= 

f XWDSN= , XDSDSN= 

: LATERAL HYDRODYNAMIC COEFFICIENTS 

CONST YPDOT= ,YRDOT= /YPO = YOR a 
YVDOT= YP = a oe 
YWP = /YWR = WYV = /YVW = 

. YDR = /CDY = 

‘ NORMAL HYDRODYNAMIC COEFFICIENTS 

CONST ZQDOT= /ZPP _ZPR = ZRR = 
ZWDOT= 200= _ZNP = ZVR = 
ZW = 'ZNV = Aa A) 

5 ZON = /ZWN = /ZDSN= (GDZa= 

‘ ROLL HYDRODYNAMIC COEFFITIENTS 

CONST KPDOT= | KRDOT= KPO = KOR = 
KVDOT= kee .KR = a 
KWP = _ KWR = KV = ™ 

i KPN = , KDB = 

z PITCH HYDRODYNAMIC COEFFICIENTS 

CONST MQDOT= _ MPP = /MPR = MRR = 
MWDOT= MQ = MVP = MVR = 
MW = , MVV = ‘MDS = \MDB = 

A MON = , MWN = ,MDSN = 

a YAW HYDRODYNAMIC COEFFICIENTS 

CONST NPDOT= , NRDOT= /NPQ = ‘NOR = 
NVDOT= NP = NR = WNVO = 
NWP = | NWR = INV = a 

: NDR = 

‘ MASS CHARACTERISTICS OF THE FLOODED auv 

CONST WEIGHT = , BOY = /VOL = XG = 
YG = , 2G = /XB = \2B = 


APPENDIX B 
DSL LISTING FOR AUV SIMULATION 


Lie 


gee@e 


, eee 


ye @@ 
gee@e 


geee 


OF RO OF OF 


+ O-NE 


aA A A 


Ix = pay = Ls 
IYZ = peLAY = yi 
L= , RHO = ,G = 

AO = ,KPROP = ,NPROP = 


INPUT INITIAL CONDITIONS HERE IF REQUIRED 


NITIAL 
INITIALIZE ALL MATRICES AND ARRAYS TO ZERO 


N = 6 

DO 2 J =1,N 
JJ= J+N 
DO 1K = 


XDOTA 
XDOTU 


H lim 


CONTINUE 
INPUT THE LINEARIZATION POINT PARAMETERS 


om | @eletealeleile) 


OOO 
© 


=O 
0 
0 


Cy 

rr 

tc 
rPrP oOo 
QQQ 
i) | 


0 
ZORD = 100.0 
INPUT THE MODEL STATES INITIAL CONDITIONS 


UM = 6.0 
i= ORO 
WM = 0.0 
PM = 0.0 
OM = 0.0 
RM = 0.0 
XPOSH = 0.0 
YPOSM = 0.0 
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,LkZ = 


,NU = 
XLITEST= 


~ * * 
e 


oF OF 


> a 


A A + a 4 


+b +t 


+ 


A OO OO Ot 


PHIM = 0.0 
THETAM = 0 
PSIM = 0.0 


INITIALIZE 


DELBOY= 0.0 


DBS= OQ. 
DBP= OQ. 
DS = 0. 
DR = QO. 
RPM = 2 
LATYAW 
NORPIT 


oO 


linoonoo 
Oe 


7 O 
OOO 


QO, 


THE CONTROLS 


DEFINE LENGTH FRACTIONS FOR GAUSS QUADUTURE TERMS 


G4(1 
G4(2 
G4(3 
G4(4 


0.069431844 
0.330009478 
0.669990521 
0.930568155 


DEFINE WEIGHT FRACTIONS FOR GAUSS QUADUTURE TERMS 


GK4(1) = 
GK4(2) = 
GK4(3) = 
GK4(4) = 
DEFINE THE 
BR(1) = 75 
BR(2) = 75 
BR(3) = 75 
BR(4) = 55 
HH(1) = 16 
Hie) = oi 
Fils Jo= Si 
HH(4) = 23. 


QO. 
QO. 
QO. 
Q. 


1739274225687 
3260725774312 
3260725774312 
1739274225687 


BREADTH BB AND HEIGHT HH TERMS FOR THE INTEGRATION 


21712 
flee 
ni fg 
SO jaz 


MASS = WEIGHT/G 
DIVAMP = DEGSTN*0 .0174532925 


RUDAMP 


THE LINEAR 


DEGRUD*0 .0174532925 


PROPULSION MODEL ~- 


ETA = 0.012*500.0/U0 
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ETA = 1.0 
RE = UO*L/NU 


CDO = .00385 + (1.296E-17)*(RE - 1.2E7)**2 

CT = 0.008*L**2*ETA*ABS (ETA) /(AO) 

CT1l = 0.008*L**2/(A0) 

EPS = -1.0+(SQRT(CT+1.0)-1.0)/(SQRT(CT1+1.0)-1.0) 
XPROP = CDO*(ETA*ABS(ETA) - 1.0) 


MASS = WEIGHT/G 

DIVAMP = DEGSTN*0.0174532925 

RUDAMP = DEGRUD*0.0174532925 

CALCULATE THE MASS MATRIX 

MM(1,1 MASS -((RHO/2)*(L**3)*XUDOT) 
MM(155 MASS*ZG 

MM(1,6 -MASS*YG 


A A A> 


MM(2,2) = MASS reales ei ED 
MM(2,4) = -MASS*ZG ae eee 
; MM(2,6) = MASS*XG - ((RHO/2)*(L**4)*YRDOT 
MM(3,3) = MASS - ((RHO/2)*(L**3)*ZWDOT) 
mM(3,4) = MASS*YG 
; mM(3,5) = -MASS*XG -((RHO/2)*(L**4)*ZQDOT) 
MM(4,2) = -MASS*ZG - ((RHO/2)*(L**4)*KVDOT) 
mM(4,3) = MASS*YG 
Mt 4,4 = IX - ((RHO/2)*(L**5)*KPDOT) 
m(4,5) = -I 
MM(4,6) = -IXZ -((RHO/2)*(L**5)*KRDOT) 
MM(5,1) = MASS*ZG 
MM(5,3) = -MASS*XG -((RHO/2)*(L**4)*MWDOT) 
MM(5,4) = -IX 
MM(5,5) = IY -((RHO/2)*(L**5)*MQDOT) 
A MM(5,6) = -IYZ 
MM(6,1) = -MASS*YG 
mMM(6,2) = MASS*XG Ree Ea L**4 )*NVDOT) 
MM(6,4) = -IXZ - ((RHO/2)*(L**5)*NPDOT) 
mM(6,5) = -IYZ. 
MM(6,6) = IZ - ((RHO/2)*(L**5)*NRDOT) 


LAST = N*N+34N 

DO 20 M = 1,LAST 

WKAREA(M) = 0.0 
20 CONTINUE 


IER = 0 

IA = 6 

IDGT = 4 

WRITE( 8,400)((MM(I,J), J = 1,6),I = 1,6) 

CALL LINV2F(MM,N,IA,MMINV,IDGT,WKAREA, IER) 

WRITE( 8,400)((MMINV(I,J), J = 1,6),I = 1,6) 
00 FORMAT (6E12.4) 


CALCULATE THE A MATRIX FOR THE LINEAR MODEL 


A 


RHO/ 2*L**2% ( XVDR*VO*DR+KXWDS*DS*WO+RXWDB/2*DBP*WO + ... 
2*U0* (XDSDS*DS**2 + XDBDB/2*DBP**2 + XDRDR*DR**2))+ ... 
RHO / 2*L**3*XQODSN*OQO*DS*EPS+RHO/2*L*%*2* (XWDSN*WO*DS +... 
DE RDEDSN*UONDS**2 )*EDS+RHOML¥®2 *UOAKPROP+RHO/2*L** 3% 


Gi) BuO /Jarsaae (Ropervorberaapssosmiossnbe/> DBPATO. 
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A(1,2) 
A(1,3) 
A(1,4) 
AGS? 


il 


Il 


Alo) 
A(1,11)= 
A(2,1) 
B33) 
a 
A(2,4 
ACze 5) 
AGZ op) 


Oa 
BAZ, il 


BX 3,1) 
A(3,2 
n3:3) 
A(3,4 
A(3,5) 
AXSo) 


ene 
Ate,i1 


A(4,1) 


Hou a i 


A(4,2) 
A(4,3) 
A(4,4) 
A(4,5) 
A(4,6) 
A(4,10)= 
CG TA 


A(5,1) = 


A(5,2) 
Abang} 
AGse >) 


XQDB/2*DBS*Q0+RHO/2*L**2*XWDB/2*DBS*WO+RHO*L**27U0%. . . 
XDBDB/2*DBS**2 

MASS*RO+RHO/2*L**3% ( XVP*PO+ XVR*RO) + RHO/2*L**2%* ... 
(2*XVV4VO + XVDR*UO*DR 

-MASS*QO + Mee eaADEP Gn Oe DR IE Mee tet ae seen 
XWDS*DS*U0+ ( XWDB/2*DBP+XWDB/2*DBS )*UO0 +XWDSN*UO*DS*EPS ) 
-MASS*YG*Q0-MASS*ZG*RO+ RHO/2*L**4%(2*XPP*PO+XPR*RO)... 
+ RHO/2*L**3% (XVP*VO ) 

-MASS*W0+2*MASS*XG*Q0 mS A RN Ot a 
aps Lica alee CSU HY AOE) Ue DB/2*DBP*U0 )+RHO/2%* ... 
L**3%XQODSN*UOXDS*EPS+RHO/ 24 L**3%XODB/ 2*DBS*U0 
MASS*V0+2*MASS*XG*RO-MASS*ZG* PO+RHO/ 24L**4% ( 2*XRR*RO... 
+ XPR*PO) + RHO/2*L**3%*(XVR*VO + XRDR*UO*DR) 

-(WEIGHT - BOY)*COS(THETAO) 


-MASS*RO+RHO/ 2*L**3% (YP*PO+YR*RO ) +RHO/ 2*L**2*(YV*VO+... 
2*YDR*UO*DR 

RHO/ 2*L**3*YVQ*%QO+RHO/ 24L**2% (YVAUO+YVWAWO ) 

MASS*PO+ RHO/2Z*L**3% (YWP*PO+YWR*RO ) +RHO/ 2*L4A%* 2*YVWAVO 
MASS*WO-MASS*XG*Q0+2*MASS*YG*PO+RHO/ 2*L**44YPO*O0+... 
RHO/2*L**3% (YP*UO+ YWP*WO 

-MASS*XG*PO -MASS*ZG*RO+RHO/ 2*L**4% (YPQO*PO+YOR*RO) +... 
RHO/ 2*L**3*YVQ*VO 
-MASS*U0+2*MASS*YG*RO-MASS*ZG*Q0+RHO/2*L**4*YOR*QO +... 
RHO/2*L**34%(YR*UO + YWR*WO 

(WEIGHT - BOY)*COS(THETAO )*COS(PHIO) 

~(WEIGHT - BOY)*SIN(THETAO)*SIN(PHIO) 


MASS* O+RHO/ 2*L**3%Z0*00+RHO/2*LA*2% (ZWAWO+2*U0*ZDS*DS.. . 
+2*U0*ZDB/2*DBP+(ZWN*WO+2*ZDSN*UOADS ) *EPS ) +RHO/ 2*L**3%... 
ZON*QO*EPS+ RHO/Z2*4L**2*%24U0*ZDB/2*DBS 
-MASS*PO+RHO/2*L**3% (ZVP*PO+ZVR*ARO ) +RHO/ 2*L*X2*24ZVVAVO 
RHO/ 2*L**2%*(ZW*U0 + ZWN*UOX*EPS ) 
-MASS*VO-MASS*XG*RO+2*MASS*ZG*PO+ RHO/2*L**4%(2*ZPP*... 
PO + ZPR*RO) + RHO/24*L**3*ZVP*VO 
MASS*UO - MASS*YG*RO+2*MASS*ZG*QO+RHO/ 24LA*3*ZO*UN +... 
RHO/ 24L**3*ZON*UO*EPS 
-MASS*KG*P0-MASS*YG*Q0+RHO/ 2*L**4% (ZPR*PO+2*ZRR*RO)+... 
RHO/ 2*L**3*ZVR*VO 
eee - Boe eS PERG COS CHE 

WEIGHT - BOY)*SIN(THETAO)*COS(PHIO 


MASS*YG*QO + MASS*ZG*RO + RHO/2*L**4*(KP*PO + ... 

KR*RO ) +RHO/ 2*L**3% (KV*VO+2*U0* (KDB/2*DBP-KDB/2*DBS) )+... 
RHO/ 2*L**3*UO*KPROP+ RHO/2*L**4*KPN*PO*EPS 

ete ea + RHO/2*L**4*KVQO*00 + RHO/2*L**2%*(KVAUO... 

+ KV 

-MASS*ZG*PO + RHO/2*L**4%(KWP*PO + KWR*RO) + ... 
RHO/2*L** 34KVW*VO 

-IXY*RO + LXZ*00 - MASS*YG%VO - MASS*ZG*WO + ... 
RHO/ 24L**5*KPO*QO + RHO/2*L**4% (KPXUO+KWPAWO 

-IZ*RO + IY*RO + 2*IYZ*QO + IXZ*PO + MASS*YG*UO +... 
RHO/ 2*L**5%* (KPQ*PO + KQR*RO ) + RHO/2*L**4*KVO*VO 
-IZ*QO + IY*QO - 2*IYZ*RO + MASS*ZG*UO +... 
HO/Z*L**5*KOR*OO + RHO/ 2°L**4* ( KR*UO+KWR*WO ) 

- (YG*WEIGHT-YB*BOY )*COS(THETAO)*SIN(PHIO)... 

- (ZG*WEIGHT -ZB*BOY ) *COS ( THETAO ) *COS ( PHIO 

- (YG*WE IGHT-YB*BOY )*SIN( THETAO) *COS(PHIO)... 

+( ZGAWEIGHT-ZB*BOY ) *SIN( THETAO ) *SIN( PHIO 


ee + RHO/2*L**44MO*00 + RHO/2Z°L**3°MWAHO t.. . 
/ 


RHO/ 2*L**3*U0* (MDS*DS+MDB/2*DBP) + RHO/2*L**4*MQN*Q0%... 
EPS + RHO/2*L**3*%(MWN*AWO + 2*MDSN*UO*DS)*EPS+... 
RHO/2*L**3*U0*MDB/2*DBS 

MASS*XG*PO + MASS*ZG*RO + RHO/2*L**4*(MVP*PO + ... 
MVR*RO) + RHO*L**3AMVVAVO 

-MASS*ZG*O0 + RHO/2XL**3AMWAUO + RHO/2*L**3*MWN*UO*EPS 
-IX*RO + IZ*RO - IYZ*QO - 2*IXZ*PO + MASS*XG4VO +... 
RHO/2*L**5*(24MPP*PO + MPR*RO) + RHO/2*L**44*MVPAVO 
IXY*RO -IYZ*PO - MASS*XG*UO -MASS*ZG*WO + RHO/2%... 
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NO 
16a) Sa] 
= OV 
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a il 


H 
~~ 
O* 
_ 
~~ 
1 


» 
- 
O* 
Ov 
~~ 
1 


} 


= -IY*PQ + IX*PO = 2*IXY*OO - IXZ 
= IYZ*PO -1IXZ 


= U0*COS(PSIO)*COS (THETAO 


oi 


L**QAMO*UO + RHO/2*L**4X*MOQNXUOXEPS 

~IX*PO + IZ*PO + IXY*QO + 2*IXZ*RO + MASS*ZG*VO +... 

RHO/ 2*L**5% (MPR*P0+2*HRR*RO ) +RHO/2*L**4*MVRAVO 

XG*WE IGHT- XB*BOY ) *COS (THETAO Saas 
XG*WEIGHT-XB*BOY )*SIN(THETAO)*COS(PHIO) - 
ZGXWEIGHT-ZB*BOY ) *COS (THETAO 








= -MASS*KG*RO + RHO/2*L**4*(NP*PO +NR*ROQ) + RHO/2%... 


L**3% (NV*VO+2*NDR*UQ*DR ) +RHO*L**3*U0*NPROP 
ae + RHO/2*L**44NVO*00 + RHO/2*L**3%(NVAUO+... 


= MASS*XG*PO + MASS*YG*%QO + RHO/2*L**4% (NWP*PO+NWR*RO)+... 


RHO/ 2*L**34NVW*VO 


= -IY*QO + IX*QO + 2*IXY*PO +IYZ*RO + MASS*XG*WOt... 


RHO/2*L**5*NPO*QO + RHO/2*L**4% (NP*UO+NWP*WO 
RO + MASS*YG*WOt+... 
RHO/2*LA%5% (NPQ*PO+NOR*RO) + RHO/2*L**42NVOQAVO 
QO - MASS*XG*U0 -MASS*YG*VO + ... 
RHO/2*L**5%NOR*QO + RHO/2*L**4%(NR*UO +NWR*WO) 
(XG*WEIGHT-XB*BOY) *COS (THETAO ) *COS (PHIO) 
- (XGAWEIGHT - XB*BOY ) *SIN(THETAO)*SIN(PHIO) +... 


(YG*WEIGHT-YB*BOY ) *COS(THETAO ) 

COS (PSIO)*COS (THETAO 

COS (PSIO)*SIN(THETAO coum enr a) ~ ee (PHTO) 
COS (PSIO)*SIN(THETAO)*COS(PHIO) + SIN(PSIO)*SIN(PHIO 








een oer aa ka oon yes oriene = VO"SIN(PS10)*... 
SIN(PHIO) - WO*COS(PSIO)*SIN(THETAO)*SIN(PHIO) + ... 
WO*SIN(PSIO)*COS(PHIO) 


= -UO*COS(PSIO)*SIN(THETAQ) + VO*COS(PS10)*COS(THETAO)*. . . 


SIN(PHTO) + WO*COS(PSIO) 
J aH ~ VOXCOS(PSIO0)*COS(PHIO) - WO*SIN(PSIO)*... 
SIN(THETAO)*SIN(PHIO) + WO*COS(PSIO)*SIN(PHIO) 


Sri came) Costin TAQ 
SIN(PSIO)*SIN(THETAO Se > Ee DoT aeaicn 4 
SIN(PSIO)*SIN(THETAO )*COS(PHIO) =- COS(PSIO)*SIN(PHIO 

Soo eee TN oar ON AST Tribes =e VO<COS(PS10)*... . 
SIN(PHIO) - WO*SIN(PSIO)*SIN(THETAO)*SIN(PHIO) - ... 

WO*COS(PS1I0)*COS(PHIO) 


COS ( THETAQ )*COS (PHIO 








HK 


“UO*SIN(PSIO)*SIN(THETAO) + VO*SIN(PSIO)*COS(THETAO)*... 


SIN(PHIO) + WO*SIN(PSIO)*COS(THETAO )*COS(PHIO) 
2 ee ee ))* 

eS) - VOXSIN(PSIO)*COS(PHIO) + WOXCOS(PSIO)*... 

SIN(THETAO)*COS(PHIO) + WO*SIN(PSIO)*SIN(PHIO) 


-SIN(THETAO 

Seo eaee ac cates 

COS (THETAO )*COS ( PHIO 

VO*COS (THETA ) *COS ( PHIO ) -WO*COS (THETAO ) *SIN(PHIO) 
ee earn ace Se AO) SINK PHLO) =... 
WO*SIN(THETAO )*COS(PHIO) 


1.0 
FE EG a can TRET ABS 
COS (PHIO) *TAN ( THETAO - 
O0*COS(PHIO)*TAN(THETAO) - RO*SIN(PHIO) *TAN(THETAO) 
OOXSIN( PHIO)/COS( THETAO ee iacel ect Taal sie 
RO*COS ( PHIO 


COS(PHIO) 
-SIN(PHIO) 
-QO*SIN(PHIO) - RO*COS(PHIO) 


Be ong) cog theeaos 








/COS (THETAO )*1.0/COS( THETAO 


COS ( PHIO)/COS(THETAO 

Q0*COS (PHIO)/COS(THETAO ) -RO*SIN(PHIO)/COS(THETAO ) 
OO*SIN( PHIO )/COS ( THETAO Sanna ees 

RO*COS ( PHIO)/COS (THETAO ) *TAN ( THETAO 
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SIN(PSI0)*COS(THETAO) - VO*SIN(PSIO) SIN(THETAO)*... 


OO OE 


+ 


oO OF OO OF 


60 


80 


+ OOF OF 


30 
40 


WRITE (10,200) ((A(1,d)pg=1,12),I=1,22) 


CALCULATE THE B MATRIX 


w 


RHO /2*L**3*XRDR*U0*RO+RHO/ 2*L**2% (XRDR*UO*VO+U0*%*2%... 
2*XDRDR*DR) 

UO*QO0*XQDB/2 + UO*WO*XWDB/2 + U0**2*XDBDB*DBS 
UO*QO*XODB/2 + UO*WO*XWDB/2 + U0**2*XDBDB*DBP 


XODSN*U0*QO*EPS + RHO/2*L**2% (XWDSN*U0*WO + 2*XDSDSN*.. 
UO**2*DS )*EP 

RHO/ 22s =2 0) 2 CD00 ener 
SIN (THETAO ) 


RHO / 2*L**2*YDR*U0**2 
~COS(THETAO)*SIN(PHIO) 


U0**2*ZDB/2*RHO/2*L**2 
U0**2*ZDB/2*RHO/2*L**2 
UO**2*ZDS*RHO/2*L*X2 + RHO/2*L*X2*ZDSN*UO**2*EPS 
~COS (THETAO )*COS (PHIO) 


~RHO/2*L**3*U0**2*KDB/ 2 
RHO/2*L**3*U0**24KDB/2 
~YB*COS(THETAO)*COS(PHIO) + ZB*COS(THETAO)*SIN(PHIO) 


RHO / 2*L**3*U0**24MDB/2 

RHO /2*L**3 *U0**24MDB/2 

RHO / 2*L**3% (U0**2*4MDS+MDSN*UO**24EPS ), 
XB*COS (THETAO)*COS(PHIO) + ZB*SIN(THETAO) 


RHO/2*L**3*NDR*U0**2 
~XB*COS(THETAO)*SIN(PHIO) - YB*SIN(THETAO) 


9,300)((B(I,J),J=1,6),I=1,6) 


™~ ~~ = ~~ 
il 


= “ “= ~ 
| lott tt tt 


—_ «Ss 


PO lm yO i gl i, Pe fm 


no NnmMm AHP WWWW YON FE 


~~ 


~~ ~S 


OW wWWWwW WWW WnWWW Www ww 
AM DAPWNMH DAWNMH ADHWNH Ate AM 
Nee es ee en es es ew weer 


rn, 


= 
vw 
ti 
J 
m7 
-~_ 


FORMULATE THE A AND B MATRIX FOR STATE SPACE REPRESENTATION 
MULTIPLY MMINV AND DF/DX 


DO 80 I = 1,6 
DO 70 J = 1,6 
SUM = 0.0 
DO 60 K = 1,6 
SUM = SUM + MMINV(I,K)*A(K,J) 
CONTINUE 
AA(I,J) = SUM 
CONTINUE 
CONTINUE 


MULTIPLY MMINV AND DF/DZ 


DO 50 I = 1,6 
poOe40mse= 7,12 
SUM = 0.0 
DO 30 K = 1,6 
SUM = SUM + MMINV(I,K)*A(K,J) 
CONTINUE 
AA(I,J) = SUM 
CONTINUE 
CONTINUE 


DO Sitesi 
Dower gd. =a 
REC) er) 
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UO*OO*XODS + UOAWOXXWDS +U0**2%2*XDSDS*DS+RHO/2*L*43%... 


CONTINUE 
CONTINUE 


A +O 01 


WEL (10; ee yoo 2) LL, be) 


200 FORMAT( 6 
* 
z MULTIPLY MMINV AND DF/DU 
* 
DONO — aac 
DO 100 J = 1,6 
SUM = 0.0 
DO 90 K = 1,6 
SUM = SUM + MMINV(I,K)*B(K,J) 
90 CONTINUE 
BB(I,J) = SUM 
100 CONTINUE 
110 CONTINUE 
* 
WRITE( 9 300) ((BB(I,J),J=1,6),1=1,6) 
300 FORMAT (6E12.4) 
* 


DO 405 I = 1,6 

READ aan GKK(I,J), 
405  WRITE(3,401)(GKK(I,J), 
401  FORMAT(3E20.10) 


* 


DERIVATIVE 
NOSORT Be 


» 
A**KALINEAR MODEL ARRAAAAKAAAAKAAKARARRAAKKKRRRRAAKK AKER AKA RR RARKAKKRRR 


* WRITE(8,600) 
00 FORMAT (14HENTERED DERIV.) 


; CALCULATE BB*U PART OF XDOT = AA*X + BB*U 
DO 10 J = 1,6 
OF. 0 


=e 
SUM = SUM + BB(J,K)*UMOD(K) 
15 CONTINUE 
XDOTU(J) = SUM 
10 CONTINUE 
* CALCULATE AA*X 
DomZiag= 1,12 


SUM = 0:0 
DOm25 Kk = 1,12 
SUM = SUM + AA(J,K)*X(K) 
25 CONTINUE 
XDOTX(J) = SUM 
21 CONTINUE 
x AA*X + BB*U 


CALCULATE Ae) 
DO 3 


1 
epOT(3) & = XDOTX(J) + XDOTU(J) 
aul CONTINUE 
DOwsS J ial 2 
DOL me SDOTX (J) 
35 CONTINUE 


oO Il 


UDOTM = XDOT(1 
VDOTM = xXDOT(2 
WDOTM = XDOT(3 
PDOTM = XDOT(4 
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QDOTM = XDOT(5 
RDOTM = XDOT(6 
XDOTM = XDOT(7 
YDOTM = XDOT(8 
ZDOTM = XDOT(9 
PHMDOT= XDOT(10 
THETMD= XDOT 12 


* WRITE (8,600) 
INTEGRATE XDOT TO GET THE STATE VECTOR X 


UM =INTGRL(6.0, UDOTM 


VM= INTGRL(0.0, VDOTM 
WM= INTGRL(0.0, WDOTM 
PM= INTGRL(0.0, PDOTM 
OM= INTGRL(0.0, QDOTM 
RM= INTGRL(O.0, RDOTM 
XPOSM = INTGRL(O.0, XDOTM 
YPOSM = INTGRL(0.0, YDOTM 
ZPOSM = INTGRL(0.0, ZDOTM 
PHIM = INTGRL(0.0, PHMDOT 
THETAM = INTGRL(0.0, THETMD) 
, PSIM = INTGRL(0.0, PSMDOT) 
* 
X(1) = UM 
X(2) = VM 
X(3) = WM 
X(4) = PM 
X(5) = OM 
X(6) = RM 
X(7) = XPOSM 
X(8) = YPOSM 
X(9\ = ZPOSM 
X(10) = PHIM - 
X(11) = THETAM 
A X(12) = PSIM 
ZDEPTH = ZORD - #(9) 
THMANG = X(11)*57.3 
DRM = UMOD(1 
DBSM= UMOD(2 
DBPM= UMOD(3 
DSM = UMOD(4 
DRPM= UMOD(5 
DBOY= UMOD(6 
* 
* 
AAKAKKCONTROL LAWAAAAAAAAKAKAARAKAAKAKAKAKAAARKAAKAKAKAKKAKKKAAKRARARRAKK AKA 
* 
* 
* DBS = UMOD(2 
* DBP = UMOD(3 
x DS = UMOD(4 
DBS = -(GKK(2,1)*U + GKK(2,2)*V + GKK(2,3)*W + GKK(2,4)4P +. 
GKK(2, 3)39 + GKK(2,6)*R + GKK(2,7)*XPOS + GKRK(2, 8) *YPOS- re 
GKK(2,9)*ZP0S + GRR(Z, eit + GRR (2 1) ATHETA + 
GKK(2,12)*PSI + GKK(2.13)*WM + GKK(2,14)*OM + GKK(2,15)*%... 
ZPOSM + GKK(2,16)*THETAM + GKK(2,17 ote: 2) + Chi sy 
UMOD(3) + GKK(2,19)*UMOD(4) ) 
DBP = -(GKK(3,1)*U + GRK (3 2)*V + GKK(3,3)4*W + GKK(3,4)%P +. 
GKK 3 +3)59 + GKK(3,6)*R + GKK(3,7)*XPOS + GKK(3, 8) *YPOS Ba 
GKK(3,9)*ZP0S + GERRI 3, coe + ma 11)*THETA + 
GKK(3,12)*PSI + GKK(3,13)*WM + GKK(3 (14) 3 + GKK(3,15)*... 
ZPOSM + GKK(3,16)*THETAM + GKK(3,17)*UMOD(2) + GKK(3,18)*%. 
UMOD(3) + GKK(3, 19 ) *UMOD D(4 
DS = -(GRK(4,1)4U + GKK(4 2) RV + GKK(4 2 + GKK(4,4)*P +. 
ees + GKK(4,6)*R + GKK(4,75*XPOS + GKK(4,8)*YPOS +. 
GKK(4,9)*ZPOS + GKK(4,10)*PHI + GRK(4,11)*THETA + ... 
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A A + 


* 


GKK(4,12)*PSI + GKK(4,13)*WM + GKK(4 i + GKK(4, 15)* 5. 
ZPOSM + GKK(4,16)*THETAM + GKK(4,17)*UMOD(2) + GKK(4,18)* 
UMOD(3) + GKK(4,19)*UMOD(4) ) 


PUT IN STERN AND BOW PLANE STOPS 


IF(ABS(DBS).GT.0.6) THEN 
DBS = Q. 6*ABS (DBS) /DBS 


ENDIF 
TF (ABS (DBP) , GT.0.6) THEN 
DBP = 0.6*ABS(DBP) /DBP 


ENDIF 

TF (ABS (DS) ; GT: i.) THEN 
0.6*ABS(DS)/DS 

ENDIF 


k 
AAAKKANON-LINEAR MODELAAARAAARARAKAKAKKAKARAARRRRAKARARKAAKAKAKRARRR RR 


* 


* 
* 
* 


or Oo a 


O 
O 


A a a Hh ON 


PROPULSION MODEL 


SIGNU = 1.0 
IF a PET oO mSnGhU = =1.0 
ABS (U) . LESMUTEST\) U = X1TEST 
SIGNN = 1. 
IF (RPM.LT. “0 0) SIGNN = -1.0 
ETA = 0.012*RPM/U 
RE = U*L/NU 


CDO = .00385 + (1.296E-17)*(RE - 1.2E7)**2 

et = 0. O0B*L**2 ETA*ABS (ETA) / (AO) 

et =.0.008*L**2/ (A0 

EPS = -l. 0+SIGNN/SIGNUX (SQRT(CT+1. 0)-1.0)/(SQRT(CT1+1.0)-1.0) 
XPROP = CDO*(ETA*ABS(ETA) - 1.0) 


CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
INTEGRATE USING A 4 TERM GAUSS QUADUTURE 


LATYAW = 0.0 
NORPIT = 0.0 
DO 500 K = 1,4 
UCF(K) = SORT ( (V+G4(K) ARAL) **2 + (W-G4(K)*Q*L)**2) © 
IF(UCF(K).GT.1E-10) T 
TERMO = (RHO/ Lae soit oe en? 7 
CDZ*BR(K) *(W-G4(K) *Q*L) **2 
TERM1 = TERMO* as oe verti 
TERM2 = TERMO*(W-G4(K)*Q*L /UCE(K 
LATYAW = LATYAW + TERM1*GK os AL 
NORPIT = NORPIT + TERM2*GK4(K)*L 
END IF 
CONTINUE 


FORCE EQUATIONS | 


surge FORCE 


FP(1) = MASS*V*R - MASS*W*O + MASS*XG*Q**2 + MASS*KG*R**2-. 
ees = MASS*ZG*P*R + (RHO/2)*LA*4%( RPPAP*AZ —— 
£90 Bone? + XRR*R**2 + KPR*P*R) PRL /2)®LE*3%(XWORW*O +... 
XVPx *PEKVRAVZRIUSOS (XQDS*DS+KQ Bee acer USR*DR ae, 
cs RRQ” + RWW *2 + XVDR*USVADR + UAW... 
XWDS*DS+XWDB/ 2*DBP ) +U**2>* ( aT 
XDRDR*DR**2) )- (WEIGHT -BOY)*SIN (THETA) +(RHO/2) )ALAKZX 
XODSN*U*Q*DS*EPS+ (RHO/2) *L**2* (XWDSN*UAW 
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DS+KDSDSN*U**2*,.. 


yo a 


a a > a A 


A A 


MO OO OO 


00 


A Ot OF OY 


DS**2)*EPS + (RHO/2) XL¥*24U**2*XPROP+RHO / 2ALA*ZAURQA ere 


eee /2*DBS +RHO / 2*LAX2°U**2*XDBDB/ 2*DBS**2+ : 
O/ 2*L*X2*XWDB/2*DBS*UAW 


sway FORCE 
-MASS*U*R + MASS*XG*P*Q + MASS*YG*R**2 - MASS*ZG*O*R Fick 


FP(2) = 


IEAUAR)+-yvgeynges Wes YOR*OAR ) +(RHO/ 2) *L**3% (YP 
YR*UAR + YVOAVAQ + ¥ PARP + SHR AN ER) + (RHO/2)*L**2* 
(YvAuAV + YVWAV 

COS (THETA) *SIN(PHT) 


heave FORCE 


EP(3) = 


MASS*U*Q - MASS*V*P - sore - MASS*YG*O4R + 
“MASS*ZG*P**2 ate ey een + (RHO/2) *L**4%(ZPPAPAXZ +. 
ZPR*PAR + ZRR*R*X*X2) + (RHO ALAAZ*(ZOAUZO + ZVP*VAP +. 


ZVRAVAR) +(RHO/ 3) ERROR Sea + ZVVAVAA2 + i . 
PHI)+. 
(ZWNXUAW +ZDSN*... 


eer eee NONOREI TS (NECGMTS BOY )*COS (THETA )*COS 
(RHO/2)*L**3 ZON*U*O “EPS + (RHO/2) L**2 
U*42505) SEPS+ RHO/25 LAX2AU*X2AZDB/ 2*DBS 


ROLL FORCE 


FP(4) = 


asst ice +IY*Q*R -IXY*PAR +IYZ*Q4*2 -IVYZ*R*AZ +I XZ*PAO 
MASS* 


AP +,., 


W +YDR*U**24DR) -LATYAW +(WEIGHT-BOY)*... 


Weare 


GXU*Q -MASS*YGAV*P -MASS*ZGAW*P+(RHO/ (2) *L**5*(KEQ®. . 


+ KOR*O*R) +(RHO/2)*L**4*%(KPAUXP +KRAUAR + KV cuee) 4 +. 


ee + RWRAW*R) +(RHO/2)*L**3%(KVAUAV + KVWAVAW) + 
(YGAWEIGHT - YB*BOY) }*c08 (fueta) *cOs Pur) 

ZB*BOY )*COS(THETA)*SIN(PHI) + (RHO/2)*L Pen eee ee 
(RHO/2)*L**3%U**X2*KPROP +MASS*ZG*U*R+ 1... 

RHO /2*L**3%U*%2% (KDB/ 2*DBP-KDB/24DBS) 


PITCH shen Gr 


FP(5) 


= -IX*P*R +IZ*P*R Eagan “IYZ*P*Q -IKZ*XPAXZ +I XZ*RAKZ 


Tee Canon, MASS*XG*V*P + MASS*ZG*V4R - MASS*ZG*W* 


(ZG*WEIGHT - 


Se 


e @ 


ees 
RHO/2)*L*%5%(MPPAP**2_ +MPR*PAR HHIRRSR**2) + (RHO/2 ) yALEAgE. | 
MO*UXQ + MVP*VAP + MVR*VAR) + (RHO/2)*LA*3*(MWAU"W +... 
MVVAVA 24U%*2% (MDS*DS4MDB/2 DBP) )+ Soe = (AGHWEIGHT 


XB*BOY )*COS ( THETA) *COS ( PHI 
(RHO/2)*L**3% (MWN 
tena) oepeee (ZGAWEIGHT-ZB*BOY ) *SIN( THETA ) 


YAW FORCE 
FP(6) = ~IY*P*Q +IX*PAQ +IXY*P*A2 ~IXY*Q**2 +IYZ*PAR SIEZAQAR =. 


MASS*XG*U‘R + MASS*XG*WAP - MASS*YG4V4R + MASS*YG*W 


(RHO/ (2) *LAA5% (NBO*P*D + NOR*O*R) +(RHO/2)*L**4* (NPAURPH, 


U*W+MDSN*U**24DS ) EPS+ RHO/2AL¥#3*. , 


NR*U*R + NVQ*VAQ +NWEAWAP + NWR wee) +(Rio/2) = AR3*(NVA... 


UXV + NVWAV4W + NDR*U**24DR) - LATYAW + (XG*WEIGHT - 
KB*BOY) *COS (THETA) "SIN (PHL) +(G*NEIGHT)*SIN (THETA) «. 
+ (RHO/2) *L* 3%UX* 2*NPROP-YBABOY*SIN (THETA) 


TPCZe RET | .0)THEN 


END iF 


E ner 500) (FP(I), I = 1,6) 


NOW COMPUTE THE F(1-6) FUNCTIONS 


DO 600 
DO 600 


z = 1, - 


ee 
Gy) J) = MMINV(J,K)*FP(K) + F(J) 


CONTINUE 
THE LAST SIX EQUATIONS COME FROM THE KINEMATIC RELATIONS 
FIRST SET THE DRIFT CURRENT VALUES 
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UCO 
VCO 
WCO 
INERTIA 


= 


+ 


F(8) = 


+ 


Bia) = 


F(10) = 
F(11) 
F(12) 


+ He A+ 


Rn Ze 2 
FORMAT ( 
Z= 2 


4 A OF OO OO 
© 
© 


UDOT 
VDOT 
WDOT 
PDOT 
QDOT 
RDOT 


MOovVv=aacG 
ou i wee a 
tei 
a 
- 


mS 
3.8, 
© 
W" 

i 


PHI = I 
THETA = 


| 
OOO 
OOO 


L POSITION RATES F(7-9) 


UCO + U*COS(PSI)*COS (THETA) 


+ Vx 
eae - Sr EST eeo ha 
ae 


V*(COS(PSI)*SIN(THETA)*. . 
W* (COS (PSI) *SIN (THETA) *. Re 
COS(PHI) + SIN(PSI)*SIN(PHI 


VCO + UXSIN(PSI)*COS (THETA) + V*(SIN(PSI)*SIN(THETA)*. . . 
ee} + 605 (Baie oS (gat) + W*(SIN(PSI)*SIN(THETA)*... 
COS(PHI) = COS(PSI)*SIN(PHI 


WCO - UXSIN(THETA) +V*COS(THETA)*SIN(PHI) +W*COS(THETA)*... 
COS (PHI) 


EULER ANGLE RATES F(10-12) 


P + Q*SIN(PHI)*TAN(THETA) + R*COS(PHI)*TAN(THETA) 
Q*COS(PHI) - R*SIN(PHI) 
O*SIN(PHI)/COS(THETA) + R*COS(PHI)/COS (THETA) 


0. ,1.Q)WRITE (9,500)(F(I), I = 1,12) 
E12.4) 

el ; 
F(2 

F(3 

F(4 

F(5 

F(6 

Fee 

F(9) 

= F(10 

= F(1l 

= F(12 
GRL(6.0,UDOT 
GRL(0.0,VDOT 
GRL(0.0,WDOT 
GRL(0.0,PDOT 
GRL(0.0,QDOT 


GRL(0.0,RDOT 
INTGRL(0.0,XDOTA) 
INTGRL(0.0,YDOT 
INTGRL(0.0,ZDOT 
NTGRL(0.0,PHIDOT) 
INTGRL(0.0,THETAD) 


PSI = INTGRL(0.0 ,PSIDOT) 


ZNEW = 
PHIANG 
THEANG 
PSIANG 


YNAMIC 


AAO 4+ + + 


-ZPOS 
PHI/0.0174532925 
THETA/0.0174532925 
PSI/0.0174532925 


IF (IFLAG.EQ.0.AND.JFLAG.EQ.0) THEN 
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CONTROL FINTIM =200.00,DELT = 


SAVE 

PRINT 
GRAPH 
GRAPH 
GRAPH 
GRAPH 


GRAPH 
GRAPH 
GRAPH 


GRAPH 


LABEL 
LABEL 
LABEL 
LABEL 
LABEL 
LABEL 
LABEL 
LABEL 
END 
STOP 


15.0*0.0174532925 
= =15.0%0.0174532925 
= -15.0*0.0174532925 
AND. -ABS (THMANG) . .GT.37.0) 
AG + 1 


.0.AND.JFLAG.EQ.0) THEN 
*Q 0174532925 


UMOD (4 )= 

UMOD(3 

UMOD(2 
oe THEN 


ENDIF 


e e a ti. e 


IF (IFLAG.GT.0.AND.ZCHG.GT.ZDEPTH) 
moo) -11.0*0.0174532925 


THEN 


UMOD(3 11.0%*0.0174532925 
UMOD (2 11.0%0.0174532925 
ENDIF 
LF (ZDEPTH. LT.3.0.AND.ABS (THMANG) .LT.4.10) 
UMOD(4)= 0.0 
UMOD(3) = 0.0 
UMOD(2Z) = 0.0 
JFLAG JFLAG + 1 
ENDIF 
De (JFLAG. GT. a THEN 
UMOD(4)= OQ. 
UMOD (3 620 
UMOD( 2 0.0 
ENDIF 


THEN 


lm, 








Ol 

.20,XPOS,XPOSM, UUM, ZPOs ZPOsMe neni eb oe 
DBS , DESI, DS , DSM, THEANG, THMANG , 

2.07 KP Os xPOoM UPUN-ZeOsr ZPOSM, W, W ,DBPM, 


Door DBSM, DS, DSM, THEANG, THMANG Oy OM 
(Gl, DE=TEK618) TT ME (NI= _ ,UN=SEC) ‘ZPOS (LI= 1,UN=FT). 
ZPOSM(LI=2,UN=FT 
(G2,DE=TEK618) eee 10; ,UN= SEC) W(LI=1,UN='FT/SEC'). 
WM ( LI=2 , UN=' FT/SEC! 


(G3 , DE=TEK618) 
(G4, DE=TEK618) 
(66 De=teKe19) 


TIME (NI=10 , UN=SEC) oo (LI= 1,UN='RAD/SEC'). 
OM(LI=2,UN='RAD/SEC 
PINE (NTE 10 ,UN SEC) THEANG(LI=1,UN=DEG)... 
THMANG (LI=2, UN=DEG) 
XPOS (UN=FT) ‘ZE05 (UN=FT) 
XPOSM(UN=FT) ZPOSM(UN=FT) 
TIME (NI=10, UN=SEC) DBS(LI=1, UN=RADIANS) . 
DBSM(LI=2, UN=RADIANS ) 
TIME(NI=10,UN=SEC) DS (LI=1,UN=RADIANS)... 
DSM(LI=2, UN=RADIANS ) 
DEPTH VS TIME 
HEAVE VS TIME 
PITCH RATE VS TIME) 
PITCH ANGLE VS rd 
ACTUAL DIVE PROFILE 
MODEL DIVE PROFILE) 
BOW PLANE ANGLE) 
(STERN PLANE ANGLE) 


G6 ,DE=TEK618 
G7 ,DE=TEK618 


(G8 ,DE=TEK618) 


G1 ,DE=TEK618 
G2 ,DE=TEK618 
G3 ,DE=TEK618 
G4 ,DE=TEK618 
G5, DE=TEK618 
G6 ,DE=TEK618 
G7 ,DE=TEK618 
(G8 , DE=TEK618) 
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AKAKKAKSUBROUTINE ETAT RAAAAAAAKAAAAAKAAAKAAKKAKKAARKKKKKKKKAAKAKKKKKAKKKKKKK 
KKKKKKKARKKAKRAKKARKKARRKAKAKK RK AAAKKAAARARKAKKKAARAKKKAKRKARAKKKKKAAAKKKAAKK 


QAAAN 


he 


LS 


AMAK 


20 
C 


PROGRAM ETAT 


IMPLICIT REAL*8(A-H,0-Z) 

See erat LA (30, “10$~ B(40, 40) .C(40, 40) ,D(40,40) 
COMMON/STATES/X(40) ,¥(40) ,U(40) ,W(40) 
COMMON/DIM/N,M,NR,NKS,EPS 

COMMON/DIM1/DT_ 

COMMON/FLAGS/IOPT 


SOUNON/OBTEH/Q(24, 24), R(24,24) 
DALAM ERS/ 1.0 


INITIALIZE ALL SYSTEM MATRICES 
Peo = 1.0E-/ 


DO 10 I=1,40 
A(T )= 3 


a 
1:NaHcl 


Oo il tl &1 


tO 

WH 

MAMHaGQ Gc 
al 


wv 
H 
es 
l 


CONTINUE 
See recone lClEN? MATRICES, INPUTS, INITIAL CONDITIONS 


CALL INPUT 
CALL MTXEXP 
CALL ROOTS 

DO 20 K=1,NKS 
GALL ek) 
CALL UPDAT(K 
CALL POUT(K) 

CONTINUE 

IF Fee 5 .EQ. , CALL OPTIMA 
IF (IOPT EO.1 CALL POUTOP 


AKAKKAKSUBROUTINE INPUT AAAAAAAAAKAKAKAAAKAAAKAAAAAKAAAKKAARARARAAAKKKKAKKER 
KAKKKKKAAKAKAKKAKAKAAAKKKAKKAAAKKAAKAAKAKAKAKAKAAKKKAAAKKAKKKKAKAKAKKKKKK 


11 


SUBROUTINE INPUT 
IMPLICIT REAL*8(A-H,0-2Z) 


SON oer A Se 40), B Ay ,40) ,C(40,40) ,D(40,40) 
COMMON/STATES/X(40) , Y ( OD, ,U(40) ,W(40 ) 
COMMON/DIM/N,M,NR, NKS, EES 

COMMON/DIM1/DT 


COMMON/ FLAGS/IOPT(5) 

Boerne epee! 34) ),R( 24,24) 

eo eee 7... 
OPEN( UNIT=6, FILE='FILE' ,STATUS='OLD' 


READ (5, 19) N,L,M,K, Mpeeaeet (1) Be 
WRITE(6,10) N,L,M,K,NKS,IOPT(1) ,DT 
READ (5,9) NAS 

WRITE(6,9) NAS 

DO 11 II=1,NAS 

READ oes) k(t. 2) 

WRITE(6,25) I,J,A(L,J 

CONTINUE 

READ (3.9) NBS 

WRITE(6,9) NBS 

DO 12 II=1,NBS 


WS) 


12 


13 


14 


39 


250 


J 
Tt gC r 


DO 35 


CONTI 


HCO. 


O.~ 


DOU TS 


a Ow 


CONTI 


O.~ 


DO 18 


aes 


END 
AKAKAKSYBROUTINE MIXEXPAXAKRAKAKRAARAKKKKAKRRKRKAKKKKRKARKKKKKKRAKKRKKKKKRARKARKKK 
RRREKKKRKRKKKKKKKKKKRKKKRKKKKKKKKKKKRKKKKKKRKRKRKKRKRKRKRKKKKKKKRKKRKRKKRRRKKKRKKKKKEK 


10 


20 


30 


SUBROUTINE MTXEXP 
IMPLICIT REAL*8(A-H,0-Z) 
COMMON/SYST/A(40 ,40},B(40,40) ,C(40,40) ,D(40,40) 
COMMON/DIM/N,M,NR,NKS,EPS 

COMMON/DIM1/DT 

COMMON /EET/5 (4040 )ah( a6 40) 

DIMENSION DD(40,40),L(50) , RHO(50,2),W(50) 


1F(1.EQ.J) GO TO 10 
0.0 


alt= Ita 
Oo 


CONTINUE 
DO 80 I=1,N 
IF(L(I).EQ.0) GO TO 80 


126 


40 


50 
60 


70 
80 


36 


100 


110 
120 


EZ 5 


> 


190 


xo5 
200 
130 


140 


DO 40 Je 1,N 
W(J)=0. 
CONTINUE 
DO 60 KK 1,N 
Y=DD(1I,KK) 
IF(Y.E 20 0) GO TO 60 
DO 50 N 
W(J)= HS) FEAR (RK, J) 
CONTIN 
CONTINUE 
DOO =1, 
DD(I, yaa) *x 
CONTINUE 
CONTINUE 
K=K+1 


CONTINUE 
RHO(I,2)=¥ 
Secs ( (RE 


MM=MM+1 

CONTINUE 

re HHLsEQ.N} GO TO 130 
GOe1O 120 


iN 
RHO(I,1)=RHO(I,2) 
CONTINUE 
GO TO 30 
CONTINUE 
DO 125 I 
DO 125 
DD(I,J) 
DO 125 
DD(I,J) 
CONTINU 
DO 135 
DO 135 
H(I,J)= 
CONTINUE 
WRITE {ex 


oO lot i 
~He OF He 
~~ O~ 
eee hire ey ey 


,J)+H(1,K)*B(K,J) 


aonmmnmne 


Olt tl 
Kei~ ~ 


J) 
90) 
5X, 'P-MATRIX' 


FORMAT /) 
5282 ((E(1,3), d=1,N),2=1,N) 


WRITE ( 
WRITE 
FORMAT(5X, 'OB-MATRIX' ,/) 

WRITE ( 260 ((H(I1, 5)! JE1,N),I=1,N) 
FORMAT (6E12.4) 

RETURN 

CONTIN 

WRITE (6, 140) MK ,EPS 

STOP 


GO TO 100 


FORMAT(1X,'MATRIX EXPONENTIAL FAILED TO CONVERGE AFTER ',14, 


1' ITERATIONS',/,1X,'CONVERGENCE FACTOR! ,E12.4) 


END 
AKRKKKSYBROUTINE ROOTSAARAAAAAKAAKKKKAKKAKKAKKARKAKKKRKAKARKR AKA RRRRAKKRR 
KARRAAKRRAKKKRAKKKRAAKKKRRAKKKRRAKKRRRAKKRRAKAKRKAAKKKRAAKAKRRRAKKRRRAKKKRR 


SUBROUTINE ROOTS 
IMPLICIT REAL*8(A-H,0-Z) 
COMPLEX*16 22 


COMMON/SYST/A(40,40), peo ,40) ,C(40,40) ,D( 40,40) 


COMMON/DIM/N,M M,NR, NKS 
COMMON/DIM1/DT 
DIMENSION W(80) ,2Z(40,40) ,WK(3200) 
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10 


iS 


20 


Zo 


30 


40 
50 
100 


120 
130 


‘DIMENSION ae ,40) 


110X, ‘IMAGINARY PART' 


Bee 


EQUIVALENCE 771 ee meal 


IZ=40 


Oo 
O 
NO 
© 
Cy 


I,J), J=1,N) 
i ae IJOB,W,RZ,1Z,WK,IER) 


1,80) 


) 12,W1,W2 


Wee 
i~ ~ Of 


= 
~ 
is 
So ~~ 
+H 
a 
nuts. ~~ re 
Beane 


0 
Te a Coe ih 
XX(2,d =Ditac (225 ae 
WRITE (6) 120)) eel ol) xx ce) 
CONTINUE 
i a a 10X,'REAL PART', 

| 5K, Lo plexgEl2.4 Loree) 
5X, 'EIGENVECTORS' ,15,5X,2E12.4 
5X,'IER AND PERFORMANCE INDEX! ,15,10X,E12.4) 
6,130) IER,WK(1) 


FORMAT 
FORMAT 
WRITE 

RETURN 





shiek REGIE ERR EXCLT (K) KARAARARRAAAR AAR AAR RAT Ret RA TA ager 


KRRREKRKRRRKKKKKKKKRARARKKKKK 


KRAARKKKKKRKKRKRKRKRKRKRRKKRKRRKRKRKRRRRRKRKRRRRKRRRKRKRKRRRRKRRERRKK 


SUBROUTINE EXCIT(K) 


IMPLICIT REAL*8(A-H,0-Z) 


eer ,40),B(40, 40) , c(40, 40), D(40, 40) 
COMMON/STATES/X(40),¥(40) ,U(40) ,W(40) 
COMMON/DIM/N,M,NR,NKS,EPS 

COMMON/DIM1/DT 

T=DT*FLOAT (K) 

U(1)=0.0 

RETURN 


END 
AKKAKASYBROUTINE UPDAT (K) RHAAAAABAAARRAR AIRE RRA ABS Aare 


KRAEREKRKKRKKRKKRKRRKRRRRRRRKKKKRER 


KRRERERRERKEKRKREKRRRRKRRRRRKRKRKRRRRKRRRRKRRRRRRRRKRRRRKRRREI 


SUBROUTINE UPDAT(K) 
IMPLICIT REAL*8(A-H,0-Z) 
coMttoh/Staee) x ,40),B 40, 
COMMON/STATES/X(40),¥(40), 
COMMON/DIM/N,M,NR,NKS, EPS 
COMMON/DIM1/DT_ 

conan 46 40) ,H(40,40) 
DIMENSION XS(40),XN(40) , $5(40), YN (40) 
T=DT*FLOAT (K) 

DO 10 I= =I, N 

XS(I)=0 


AE ,D(40,40) 
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10 


20 


AK (J 
AUC J 
AK (J 
AU CI 


alee 
Y(I)=¥5 TNE 
CONTINUE 

RETURN 


END 
AAKKARSYBROUTINE BOURW UG weieeuea wa nT ARERR AAA AAR AAA ARR ARR 


KRRAKKKAKRKRKAARKAAKRKKAKAKKRK 


100 
110 


SUBROUTINE ey 
IMPLICIT REAL*8(A-H,0-Z) 


2 ASS ,40),B(40, 40) 
COMMON/STATES/X(40),¥(40) ,U(40) ,W(40) 
COMMON/DIM/N,M,NR,NKS, EPS 

COMMON/DIM1/DT’ 

T= (eep) 

IF(K.EQO.1) WRITE (6,110) 

WRITE (6,100) T,¥(1),¥(2),Y¥(3),¥(4) 
FORMAT (5X,F10.2,5X,4E12.4) 

FORMAT('TIME' ,5X,'X1', 5X, 'X2', 5X, 'X3!,5X, 


lees 02" ,./) 


RETURN 


56(40, 0, 40), D(40,40) 


KRAKKAKKRKKKAKKAKRKKRAKRKKKRKRAKKRKKRKRKRRRKAKRKRKRRKRURKKRKERK 


END 
Aa*KAASUBROUTINE OPTIMA KKAAARAAAKARARAAAAKKAAKAAAKKKRR AAA AAKKAKRRRAARAKKKKRR 


* 
* 
* 
* 
* 
* 
* 
* 
* 
* 


GA 
KRARKARKKKAKKKKAKRKAKKKAKKARAKKRKKRKAKRKRRARAKRKAAKKE 


Pols SUBROUTINE SETS UP THE SYSTEM AND ADJOINT EQUATIONS 


MATRICES AS 
Ss = 


A -(B(R-1)BT) 
-0 -AT 


AND FINDS THE EIGENVALUES /EIGENVECTORS OF SS. 
COLCEGLING THE STABLE VECTORS AS IN POTTERS METHOD 
AND PARTITIONING, RESULTS IN THE SOLUTION OF THE 
RICCATI EQUATION FOR THE aaa STATE FEEDBACK 


INS.THIS ROUTINE LIMITS A 


SUBROUTINE OPTIMA 

IMPLICIT REAL*8(A-H,0-Z) 
COMPLEX*16 ZZ,W12,W22 
COMMON/SYST/A(40,40) Pre 40) 
fee orn /o(28, 24) ,R(24,24) 
COMMON/DIM/N,M,NR,NKS, EPS 
COMMON/DIM1/DT 
DIMENSION $S(48, 
DIMENSION W12(24,24) ,W22 
EQUIVALENCE (22(1,1),RZ(1 
[A=48 

1Z=48 


46 


0.0 
0) ((R(I,J) ,J=1,NR) ,I=1,NR) 
NDIM2=48 


CALL INVERT(R,DET,NR,NDIM1,NDIM2) 
WRITE(6,150) ((R(1I,J),J=1,NR) ,I=1,NR) 


129 


24,24). 
eae KAKKKKKKAKKKRKRRRKRAARKKKAAREKKERSK 
C(40,40) ,D(40, 40) 


48) ; TEME (24,2 43.2 z(48 4 48) ,W(96) ,WK(2400) 
)) 


* 
* 
* 
* 
* 
* 
* 
* 
* 
* 
* 


10 


20 


30 


40 


200 


220 


230 


210° 


oO 
oO 
re 
oOo 
aH 
mo 


MP (I, J)+R(1,K)*B(J,K) 


nnikah uu 


——~ 


I,JJ)-B(I,K)*TEMP(K,J) 


oO 

O 

© oO 
Qh KQHGaAR GrH-~ 


| 


JI=I+N 

Bae ,J3)=-A(J,I) 

SS(II,J)=-O(1,d) 

CALL EIGRF(SS,N2,IA,1JOB,W,RZ,IZ,WK, IER) 
WEE NOs a 

DO 50 I=1,N4,2 


T1) 
oe I2,W1,W2 
6,100 
DO 70 J=1,N2 
DO 60 I=1,N2 
S(T, b)= =REAL(Z 
I ,2)=DIMAG( 
WRITE (6,120) 
CONTINUE 
CONTINUE 
COLLECT ALL STABLE EIGENVECTORS INTO A V-MATRIX 
(USING $S(48,48)),PARTITION,AND SOLVE FOR THE 
SOLUTION OF THE RICCATTI EQUATION . 


J=0 
DO 210 ICI, N4,2 
JC=(IC+1)/2° 


LF(W(IC). “CH. up GO TO 210 


DO 200 I=1,N 
IPN=I+N 

see ve) 
W22(1,J)=ZZ(IPN,JC) 
CONTINUE 

CONTINUE 


DO 220 I=1,N 

DO 220 J=1,N 

IPN=I+N 

JPN=J+N 

SS(I,J)=REAL(W12 

SS(IPN, af =DIMAG(W1 

SS(I,JPN)=-SS(IP 

SS(IPN,JPN)=SS(I,J 

CONTINUE 

NDIM1=48 

NDIM2=96 

CALL INVERT(SS,DET,N2,NDIM1 ,NDIM2) 
FORM W22*(W12)-1=P 

DO 230 I=1,N 

DO 230 J=1,N 

TEN=1+N 

ee 

R(I 


DO 240 
DO 240 
SS(I,J) 


INVERT COMPLEX W12(N,N) 


4 2 a 
3) 


J) 


Ly 
(te 
IPN 
Sey 
1,N 
0 


I caraen 


0. 
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240 
2 


10, 
c 


260 


Zoo 


DO 240 K= 
Soe) = SsiL, J)+REAL(W22(I,K))*Q(K,J)-DIMAG(W22(1,K))*R(K,J) 


CONTINUE 
FORM GAIN MATRIX INTO THE Q ARRAY 


)+TEMP(I,K)*SS(K,J) 


COMPUTE THE CLOSED LOOP A-MATRIX 
DO 260 I=1 
DO 260 : 
A(I,J)=A(1,J)-B(1,K)*Q(K,J) 
WRITE(6,27 

DO 265 K=1 
WRITE(2,27 (Q(K,J),J=1,N) 
WRITE (6,280 

DO 285 I=1,N 

WRITE(6,275)  (A(I,J),J=1,N) 

CALL ROOTS 

FORMAT (5X, 'EIGENVALUES- -SYSTEM+ADJOINT-' ) 
FORMAT(5X, ‘EIGENVECTORS RE/IMAG' ) 

FORMAT (5X,15,10X,E12.4,10X,E12.4) 

FORMAT(5X, 'R-INVERSE!' ,/,4E12.4 
FORMAT(5X,'R-MATRIX' ,/,4E12. 4) 

FORMAT(5X,'TOTAL STATE FEEDBACK GAIN MATRIX',/) 
FORMAT (3E20.10) 

FORMAT(5X,'CLOSED LOOP A-MATRIX',/) 

RETURN 


END 
AAAAAASUBROUTINE INVERTAXAAAAAAAKAAKARAAKKAARAKKAKRAAAKAKAAKAAKAKRAKAAKAAKKRRA 
KAAKKARKAKRAKARERKAKKARKRRARARARARRAKKARARKARKARARRRAKRRRARRRKARARARKRARRRAAKRAAKK 


aan 


10 


NaY 


30 


SUBROUTINE INVERT(A,DET,N,NDIM1 ,NDIM2) 

IMPLICIT REAL*8(A-H,0-Z 
THIS ROUTINE INVERTS A SQUARE MATRIX USING 
GAUSS ELIMINATION.THE ORIGINAL MATRIX IS DESTROYED 
AND ITS INVERSE IS RETURNED AS 'A'. 

DIMENSION A(NDIM1,NDIM2) 

NDIGIT=30 

SUM=0.0 

er Vanei=1.N 

Do 1osg=1 -N 

SUM=SUM+ABS(A(I,J)) 

CONTINUE 

SUM=10.0**(-NDIGIT/2.)*SUM/N**2 

NP1=N+1 

NPN=N+N 

Dereon i=) N 

IPN=I+N 

DO 20 J=NP1,NPN 


ale a0). 0 

IF (IPN.EQ.J) A(I,J)=1.0 
CONTINUE 

DO 25 I=1, 

WRITE (6, 560) (A(1,J),J=1,NPN) 
CONTINUE 


TE (I.EQ.N) GO TO 50 


DO 30 J=IP1 : 
IF (ABS (A (1, t)). LT.ABS(A(J,1))) Med 


iF (Me 
GOe1Oe 0 


tHTets ree, 
DO 40 J=1,NPN 


131 


40 
50 


60 


70 
80 
90 


100 


26 
110 


£26 


130 
140 
900 
910 


TEMP=A(M, J) 
a s ale J) 
A(I,J =TEMP 
CONTINUE 
CONTINUE 
DE(At it). 
IF ABS (A( 
DO 60 J=IP 
A(I,J)=A(I 
CONTINUE 
DO 80 J=1,N 
TF (J .EQ 1) ne TOPSG 
DO 70 KoiP 1, 


ACI KY=A(J, "kY =A (J, I)*A(I,K) 


A(J,I)=0.0 

CONTINUE 
DET=DET*A(I,I) 
CONTINUE 
DET=(-1)**INTCH*DET 
DOmOe  1=Ik 

DO 100 J=1,N 
A(I,J)=A(1,J+N) 
CONTINUE 

DO 26 I=1,N 

WRITE (6 , 310) (A(1I,J),J=1,NPN) 
CONTIN 

RETURN 

CONTINUE 

WRITE (6,130) 

DO 120 I=1,N 


0) GOO 110 = 
.LT.SUM) WRITE(6,140) 


E 
i 
1 
(1,1) 


CONTINUE 

RETURN 

FORMAT(5X,'THE MATRIX IS SINGULAR,NO SOLUTION HAS BEEN FOUND') 
FORMAT(5X,'THE MATRIX IS ILLCONDITIONED' ) 

FORMAT(2X, 'ABEF.! ,6E12.3 

FORMAT( 2X, "ARPD. sonl2.4 


AAKAKASYBROUTIME POUTOPAXAAKKAKKAKKAKKAKAAKARAKKAKAAKAKKAARKAAKAKKAKARARKA 
KAKAKKAKRKKAAKKAKKARKRAKRARKKAAKAAKRARRAARKAKKRAKRKAAKKRARRERARAAKAAKKRARKKRARKRKKARK 


SUBROUTINE POUTOP 
IMPLICIT REAL*8(A-H,0-2Z) 
RETURN 

END 


132 


10. 


LIST OF REFERENCES 


Bane, G.L., Ferguson, J., "The Evolutionary Development 
of the Military Autonomous Underwater Vehicle,"5th 
International Symposium on Unmanned Untethered 
Submersible Technology, University of New Hampshire, 
wene 22-24, 1987, Vol. 1, pp. 60-88: 


5th International Symposium _on Unmanned  Untethered 


Submersible Technology, University of New Hampshire, 
June 22-24, 1987, Symposium Proceedings. 


Herbert, M., Thorpe, C.E., et al., "A Feasibility Study 
for a Long Range Autonomous Underwater Vehicle," 5th 
International Symposium on Unmanned Untethered 
Submersible Technology, University of New Hampshire, 
June 22-24, 1987, Symposium Proceedings. 


NCSC Technical Memorandum Zo 7 8, "SDV Simulator 
Hydrodynamic Coefficients," by.N.S. Smith, J.W. Crane 
and D.C. Summey, June 1978. 


International Business Machiens Program Number 5798-PXJ, 
"Dynamic Simulation Language/vs Language Reference 
Manual," June 1984. 


NSRDC Report 2510, "Standard Equations of Motion for 
Submarine Simulation," by M. Gertler and G.R. Hagen, 
wune 1967. 


Walker, F.L., Analysis and Simulation of An Autonomous 


Underwater Vehicle Control System, Master's Thesis, 
Wi222137, Naval Postgraduate SeEnNOO1, Monterey, 


California, June 1983. 


Amerongen, wie Van, Wellin A. J . Ten Cate, "Model 
Reference Adaptive Autopilots for Ships," Automatica, 
Great Britain, 1975, Vol. 11, pp. 441-449. 


Milliken, G.L., Multivariate Control of an Underwater 
Vehicle, Master's Thesis, MIT, Cambridge, Massachusetts, 
1984. 


Kwakernaack, H., Sivan, R., Linear Optimal Control 
Systems, Wiley Interscience, 1972. 


133 


ii 


12. 


13. 


14. 


15. 


16. 


17. 


Kaufman, H., Berry, P., “Adaptive Flight Control Using 
Optimal Linear Regulator Techniques," Automatica, Vol. 
i2, Oxtord, 19/6, Pp. >oe>, 7 co. 


Slotine, J.J.E., Tracking Control of Non-Linear Systems 


Using Sliding Surfaces, Ph.D. Dissertation, MIT, 
Cambridge, Massachusetts, May 1983. 


Yoerger, D.R., Slotine, J.J2E-,. “Rebust ~irajyecrory 
Control of Underwater Vehicles," I.E.E.E. Journal of 
Oceanic Engineering, Vol. OE-10, Number 4, October 1985. 


Landau, I.D., "A Survey of Model Reference Adaptive 
Techniques--Theory and Applications," Automatica, Great 
Britain, Vol. 10, 1974, pp. 353-379. 


Landau, I.D., "Model Reference Adaptive Controllers and 
Stochastic Self-Tuning Regulators," Transactions of 


A.S.M.E. Journal of Dynamic Systems Measurement and 
Control, New York, 1981, Vol. 103, Number 4, pp. 404- 


416. 


Newman, W.S., Hogan, N., "The Optimal Control of 


Balanced Manipulators," Robotics: Theory and 
Application, A.S.M.E., New York, 1986, pp. 177-184. 


Slotine, J.E.E., “On the Adaptive Control of Robot 


Manipulators," Robotics: Theory and Applications, 
A.S.M.E., New York, 1986, pp. 51-56. 


134 


INITIAL DISTRIBUTION LIST 
No. Copies 


Defense Technical Information Center 2 
Cameron Station 
Alexandria, Virginia 22304-6145 


Library, Code 0142 2 
Naval Postgraduate School 
Monterey, California 93943-5002 


Chairman, Code 69Hy 5 
Mechanical Engineering Department 

Naval Postgraduate School 

Monterey, California 93943-5004 


Professor D.L. Smith, Code 69Sm a 
Mechanical Engineering Department 

Naval Postgraduate School 

Monterey, California 93943-5004 


Professor R. McGhee, Code 52Mz ; 1 
Computer Science Department 

Naval Postgraduate School 

Monterey, California 93943-5004 


Professor R. Christi, Code 62Cx a 
Electrical and Computer Engineering 
Department 


Naval Postgraduate Schoo; 
Monterey, California 93943-5004 


Dr. J. Crane AL 
Head, Hydrodynamics Branch 
NCSC 


Panama City, Florida 32407-5000 


Russ Weneth, Code u25 ae 
Naval Surface Weapons Center 
White Oak, Maryland 20910 


Paul Heckman, Code 943 i 
Head, Undersea AI & Robotics Branch 

Naval Ocean System Center 

San Diego, California 92152 


3D 


10. 


as 


12. 


1s. 


14. 


aa 


16. 


17. 


18. 


Dr. D. Milne, Code 1563 
DTNSRDC, Carderock, 
Bethesda, Maryland 20084-5000 


RADM G. Curtis; USW PMS-350 
Naval Sea Systems Command 
Washington, D.C. 20362-5101 


LT Relle L. Lyman, Jr., USN Code 90G 
Naval Sea Systems Command 
Washington, D.C. 20362-5101 


LT Richard Boncal, USN 
Raynes Neck Road, RFD 2 
York, Maine 03909 


Distinguished Professor G. Thaler, Code 62Tr 
Electrical and Computer Engineering 
Department 

Naval Postgraduate School 

Monterey, California 93943-5004 


Library 

Maine Maritime Academy 
Castine, Maine 

CDR. Cox 

Marine Engineering Department 
Maine Maritime Academy 
Castine, Maine 


LT Vincent Squitegri 

SMC 2431 

Naval Postgraduate School 
Monterey, California 93943-5000 


LT Vince Stammetti 

SMC 1104 

Naval Postgraduate School 
Monterey, California 93943-5000 


136 

















S sled r 
ecient a 
ie ee ee 


Fe eet le dent tin teenier Lael 
Fee tet octal hie hae le 
ons PA Ob ab oh o Sat n 








ot ee ei ae et hee et eT ea be et ee 2 


Peet t amon 6 Othe 


eS i ee tee ee a! 
ST dia beth ol ean 
SL aatdetedehltt tt Pe en ee 
Teh tae edtieded paced aed Ae ee a 
bathed dl a he ed ee 
et uae ited ke bed ’ 













































ssl late nat siecelindd haben he Lend tapagh aed mre eet el 
De ee a Ts see oe ee Pr tyY Pies 1 Pee ae 
ad Fenda eal tiled & ow IPOS TE? Ree rie ly ye ra) 
SN id it bade deel Pan Ti ras er ty Sy, et th) oe oe 
. crite ceedalliskad a Pe es) ed Ladd ee are 
Die Le De ei hed Bed Ce te ee a ee ee et ee ed 
Fe eee ee ’ ey Te yt wi yd feaaae @.ua8s ar 
De ee ee | At thied * Cee i tte) oe te ee ae ret 
a ett nd es 4 Mot OA ht rr; a 
| ane A ete SS eset : een 
ok Ln ete) a ee ed e 
Oe oe Al dd Par i ry 
teed hee need ed Ae el P er ee 2 ae oe ee ee es, ee | 
oe de leh et lee Oo Me tyns of a, £10.08 Sete r) 
tl aed sll teh tated 1 adie dh y be rie ee See 
amend eed deen ee See i 
Bee ee ae oD) et oe ee ee) ee oe, ee ee es ry s pe'e 
lin ted ed ee Ca “8 ar te ed os rs 
Pe Al eee ee De aoh 0, a ee Pe fete 
erry) ors Mis a tate Pert « a ee ee 
er ee dd ee ee ee send z 
ee: Rated. F nse aet yn ar. 
ey lmtihine cll Andee let A ed atte ie oe | 
Cea 2 ek ke et a. Pn Ms cap Senge 
ith badona Yo rs! ee et ee ix} War 





NL) bee 
OT eT 





A ot Tt et eee 
Pe ee ee ee ae 
eet et ee a 
Poor Pet re 2) ae hd 


















ain, a htahdtendted Oe) ae ed 
Td dt Pe ee ee 
Pree ee ae eee ee ee 
s fol ad 

i) 

Ce) 


se pt daha dete lhe ek Bo Ae A ted , 
A ledentnad bl it oth dma ateend-a totaal 
Ct ee ee hae) 












































































Ca ee | Pe) Pca Ce ee ee of cd 
“pene 7) eet toh Vem mesk be FH bare 88 
Ce TT oe aol Si ies ry Pea af « hd r 
eet ee ed Te Ae oO Out met aPase 18 4 re or ee Ae Le bd Le) ial * 
Podrdegortss = 8 6a oe nat ed ? ee A Ar ee Pier 7 a? 
gl Le a 3 ote a eed Puri. of Oye wie Pid pi or e feast Cr a ee | 
ype @ ew ee a 2 od Pre Ae oe rd Phy ee ae 4 rw ei tee) + atobee A r 
ed td La Od Le ee) eed SF eke A1e Bee «8 Poagnit ae Le [ahd Ce ee ry 
ee a ee RT de oe she be Fi%, , . soes8 ) n 
frie ie 2 td tts Pra ed Te ee es cy 
Or ee eo af whdem phcted i ee ee ee ie ri 
‘“ Te et eed oe oe | ee | A do eet et 1 4 ¢ Ce ee 
ra te) en 4 o's o r 
re od ee | nT ed oprenwe Ls ry 
Ty Pie -o r Gas a ~ i) , ed 
er oe ed re) 7 . rl n 
es rT weir . oa 
ee en Sous ds ides 
ee ra Pee ee oe Pr Pte eh wet OF et re) oe et ee oe a | tet Or 
ete hatehit at ratserd .e0 oe ed Ae ed ee Lf a oP tf er ee oe 
CA dae Of of il sare & e 4 Le Lr Pre T eter ee er 16s nis Py 
Lr Ad ee A ell cree ti Cy rr -s ’ f 7 i) I 
eee Paarl OAD Ie sé i ee ee ee ee) Cate at « . eo" 8 fer 
re a- £4 #aeot Pa rt tl | a ar a a 
wr os ci or, Goer . rd rT »47¢6 ro er a ee | en . a | a ee) 
beney o#itpegt ww © © ay “eC F 
. © eotet @ a ® O eos r oo 
or ae) a ti wd 
7? . Lite J th i | « . ses ee | s 
a ad ee a s e y : 
rd Peas re) . omer ad s (ff Ce e . ¢ ff 
Pee eat © Aad " 
woe am Tie ET) Pv) 
Pe ar eet ee pre. 
ee Car ee ed ee 
yi P Pr Pee ae 
ae ss 
- Tee ee re 
Pea et ee 
5 P rey) . 
. er r 
. a a 
Ceri bd ) 
7 rrr Pe ae 21 
Pte et A ee fe ae ry rT Let Se eee 
nt 2 ee ee eevee oP ’ @ 6 © eeu f id ad a 
o @ ° ore te Cy , ‘a 8 t F | ce | s 6 see 
ee eee i) | hd he | Pe Le 7 oy s « & . 
sole @ 5 .- . id . ar Ff Pe ee ae 2 
i Aaa ae * t Cd a U ae | e4 4 a) 
ie Pe r # eee . Pe ed Pa . 4 oie is a 
Ce ee ee ee a ee ee ‘ et e ta) ¢ or Pi 2 es 
a t ne eT ee | « 7 s 5 Py 1} rr or P 
8 * " = ° Pa Pr ‘ - 7 
de N a er ee e * ery 
é , Td =w 88 ’ 1 ed a a 
ra a} LF | * é ® ry 
r ee) , PY ar . * ry . ar es 
| 1 es 6 - awe 
ob a) r oa os Py 
ca a) f@ . ad ad Fi 
er) or) es ae Pr ew . ire) P 
or] ee s - a "s a rie tt a ee ry 
7 ar I rd ee P se 
bd . ¢ rd . A o a of ry a « 
* id out . oe " D . se 6 
‘ é i Ca ” Lat f C A * 
. a A . at Pa e . 
‘ ie fe ‘ of ay - 
Cd a , . bh da ty hr Cn) 
ta ‘ © Pa e « tf 
e . ‘ Po t.fa ey, 
ry 1 en ra - é z ° 7 
F A rT A oa) o zy . Pa 
Ps 7 * F ‘ 
Fy 2° . oa ae . t 
ay o a i ‘ ra 
4 PY - y Py 6 tg de 
a r 2 ¢ ‘ 
asian a) a Pr 4 # a ¢ e 
Py OD am aL « 
5 we Fe eee on y 
; r a Pr Fi ‘ae r 
x ps é 7 fia F 
od ’ ey r ” ¢ r . ’ 
A J I a ‘ ° ’ 
od ‘ al - , Bot 
Ld ra a oI r] = . ‘ "§ oF | rd 
[ . ) ied F es r ‘ ) |) 
e ny cI A s * Py 
H co me a D ae bs or ‘ 
2 7 Go . L . * is) Fd o eu 
p P P : a = $ oT F ca oar 
7 * 2 Ca c nd * J e 4 
ie | F f Pr se “* rt ie Ad a ’ of fl 
: is . = e 7 . . r ¢ s ‘ ee 
z - 7. ra é . 

a Pd td ® Ce a s A ? ° ° fy i 
Lad 2 af bd el . hd Pan 
Ld . r) bis Tee | ' a a ° 
oy r re } - Po P ar Co 

’ ad 2 * + $ be 4s . 
Ad bs el . s bs : . i 7 
A 
* Pe r = - . 
= ° r . a Pa we 5 

= a a ots , rt 

Pd Ps 2 idee Pr c a 
rs 5 A . S rh . A , 
me ~ eo " Sy Joa r ‘ 

a ha ted ~ sl 4 t 

he f . o tor . 

* 1 ~ hed 2 
I oa y ~ S a) 
he ¢ L: 
£ ed od id a - Fed 
se ad « a ui v ’ . « @ ne 
sd ~ 7 ry Pr A J P mm 
> ; . - P e Pa F 
oe Ja O Py rT iM ’ ‘ P 7D 
ri co . * A % e ¢ iB as ee 
. > — = 1 Pi 
. é ) « 
 hepidadts: . ee oh i 
2 s ] 

, " cs sa n t ' Ps _ 
al * be Ce F Hr .? , 
re Jo ay . ‘ P se Fie? J 3 a 
. * °? 
ed n ’ a ° 
cat © ®, < 2 e é é 
he ee a ‘ Pe a o , a 

Po r ” , ; 

== & - = 7 P 

“' aad a as my ; > 4 ar 
=y © i ve . J > : ’ 1 a 
9 » * . 7 « . * F ij P A 
ed 2% © nn NM ka F 

a - 2 arr x el ¢ ’ = 

ad ace = ® A [id U os r = 
rt meyer ese Ss 7 ry > bd ve oye P P ’ 
i ah de Ad . te = ,, © tae ee | 
a on = Pr) Ps e ‘ 

Ps a — P Pont ry ™ 
DE aie th Co wey cy . y . ee J 
ee a a Ted P = ‘ 
= ce ? * Ck * ss ca » 4¢ 7 é 

, os he o apt be o 
co * o* . a @ sh ad 
tad 2 = ¢ ede Le t ] ety | J . 9 
i eh a a] ae * an) 

ny le a he : ~ fe Par 8 . 

a Ie ta 7h - ™ e ry Pad ere 
sata edicts stalin bith! Se te . ae A 

SL ae owe “thd olan Le 7 7 P Hf Py 

diet De ih tee tee) 7’ "ia a oe Pogo ra gttes, nT ° 

inten te bet Rede) (he 7% ¥ eh De eed Py — » 
a Tas be ad Date > rT e eo es ht ee Pe i. an 
ee tate he ee. Le ms Pry set © e . 5 we 

eh ee 7 et) ee a . i , «- J 
hee ert rere cee é 3 
Stl Di i tee eli « ¥ : * Lid a ] ] ' 
ee a et ey te] ” P , Oye) yew p 
Se te et Pe ee eee om bd r 
ee ee ee EB ms wre's « . * * e " ry “ 
=~ Stitt dea tie date te XT ¥ SG o Pace? 2: e°8 oe ~ - ] 
ee sdiaeleenealiliad ome « PT te Lae oe | a y 2 
ee el te ‘e thas neh Th ee dh Di a e ee ry e ie) 
heh it in li ts eth in e ows oT ee ee ry e 

etches La ] a he a a « + ie ° c ry 

1 dua dh in ho he oD Pa ied PIV - 
pi a tee Ll pw ve ty 270s »*e he y om] . ry P 

he + L i “a Fd ry 
x ed ee a “~ - ae r 
a h ls a) a ee ed Pte 2 
¥ 7 Pe ] he ri . * 
Sr e tf 
a Se eh ee td ed om « r) " 
es = of a Pt Cee y ‘ e 
vere % ‘ a > Fy’ ave 0g ® e& an) 
— o 2e"¢ - 5e . ‘ os s or r 
th Illahee eq en Bed i tin te tt et tek De Eee hee A r rr ? ran 
act teehee de, ae ’ J ron eM ded UL Fi r Py * 
ee th te te tol kd woe 1 ote re ° 
sath tnt Le Mien al hE f hn a a As ae .@ rh 
Tend ct Ie ket a dee At o'C'a @ 6 ye 4,0 7 
lh cine th Doe Di te te Reel "a> Ce 2 a 
a ee ee ee ad Es 2% “ ' ar 
bpeslipellardth, Moninde Sthcted oth Me Bh tee hte ah) Bt Le bk Ent, ’ 
wie 2s eo OER SS dl he | oy * 
leben dd lead hein dhe $ 4 ‘ . cart) 
a ee) he me ee Le 2 © ¥ o , 
aida, ‘ed Reh 2 ha D ¥ i 
ad i we 's ve LI Cd 
ee ak L ° vt 
ie ek ee ee a LL ee eles he s* ‘ 
a ea ee ay a! Py 
YS Se eres Se sd Lh . 
anil ke en ee ie es | a 
het dedicated a] at Le celia U o' 
all Ae el * ed ‘ 

eee) s ees J 
ities . . ct ee) ri 
be ee a 1 . ft, foo ae a 
A ee ee oe an ey 7 . r 
Te i ti ae as g's ev we vie 2°44 Be ’ e 7 $ 
7 the @ we al hiatal Ce eel eecln Al Dihds Snell e 
dee A bette Mette dati) he a ie a de 
ated hak ae a La a ™ Oe ae he - 7 
Cerw ewe Oe . we ak, A die date, ek ie rio _ P r 
ba iacntcttntatliden deter lliaateteith OF hha sna Detaled M " 
db tachitee te tiie heel i Ate be Ais Be de % 
rare 1s we € r® Ate ee ch dh Di tin n 
Gn dlicten ttt din te Da Aenstinededn hie Tt 2d Be Be Sh ale * p . 
wee 100re Te t ? r 
se rteteet a as a r . a . 
Steet ae Ata | s 8 ’ ‘ P ary 4 
























Pris). 
danse’ 
De Ae 


oA te 


Pea 
Pei 
* 








Tire rae ae aa ey eet 

Cs ed sees 

owes Cree ed A ee) 
ee ee ke 

« ee C20 1 ed a eee 

fe | 0 ONO a Ooh ts =808.0 8 

re a) ee eee | es 

eeu? tLe we, a) 

, i ee ty eh aL id 





ae har) 


fpstet 



































eoersF 














a ee 
hee) 9A 
Se8 O.08 





Py ees 











i he et) 


— fei A ttt 

epee ce iS 

ry art rr 
a) San* 
PE ee 


CS 





SS | 
































=e) Ou 





















a 


Fae 


NL 





oe 


SI f. 


PrP: 























Tay eT de 
i) ae) 






































Pry. wt ae ee ad 


































Cl eee ee ee) 











Peer Te ee te oe 
Se ee 
uae 2o09 rr) 
Cy ee ede 
at het te? Ta 














7 w@hete 
2” i ar 
CAN Ue ee oe 
Me es 








par 















































































thesB667 


A study of model based maneuvering contr 


i 


3 2768 000 76750 3 
DUDLEY KNOX LIBRARY 


Mt 















































ij 
| 


il] 
| 





"i 
. ar) 
a J 
S 
° ° r 
4 4 
eu 
: - 
if 
On 
] Py 
e ° 
. 
‘ 
s id 
8 
5 
Ld ‘ 
fe 
P 
, 18 
N . 
ad 
"8 
ay 1 
. J 
a 
f O 
a r 
6 
ar) 
ets 
a 
se 
s . 
3 
) 5 
r 
oa 
pI 
F 
A 
i ” 
s rd 
e ¢ 
- 
’ 
or 
® 
S F 
es 
? 
‘ 
s 
5 
, 
Ul 
en ae) 
a 
p 
r . 
[i] 
é 
P A 
UC 
*s 
Oh 
Lt 
’ 
n 
2 
LT 
. 
LT 2 
° 
, 
ny 
VT € 
5 
O 
o 
‘ 
n 
s ¥ 
, 
a 
’ 
e 





(IKI) | 


tN 





? 
a) O 
ees 
8 6 
CO) te 
or 
s 
eat 
i 
* ‘ 
i Co) 
“ft 
i 
rl D 
’ 

5 ar) 
a 
O ec 
nn) 
tos 

er 
rT 
D 
‘ 
‘ vt 
‘ 
5 “he 
. ‘ 
5 ee 
i] 
« 
‘ 
rar 
' . 
‘ tf] 
Le 
e bbe 
Pal 5 
Ld 4 
p 
ar) p 
r 
ey 
a 
ees 
A 
Ld 
eo. 
D 
A oa 
. 
Oo 


e . 
Cr 
oy 
’ 
1 
e 
s 
"9 
D 
td 
. 
Y 
' 
iT 
ff 
i 


cy 


° 
CY 
Y 
O 
e A 
e 
® 
Ye 
4 
r) 


